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I. INTRODUCTION 


A. Motivation For Sonar Development 

Yamabico-11] is one of two mobile robots available for research at the Naval 
Postgraduate School, the other being the autonomous underwater vehicle of the AUV 
research group[13]. Yamabico is perhaps better suited for use in basic robotic coursework 
for three reasons. First, it rolls in the corridor of Spanagel Hall at NPS and doesn't require 
a team of people and a swimming pool to run a mission. Second, it operates holonomically 
in two dimensions, greatly reducing the complexity of it’s motion and the related control 
problem. This allows the student to concentrate more on the basics of sensor employment 
and path planning. Third, a high level language (Mobile robot Motion control Language, or 
MM.L) already exists for the student to use for programming the robot’s motion [9,10]. 

A set of ultrasonic sonar transducers already existed on Yamabico, and had indeed 
already been used in research regarding precision navigation [7,8]. A set of functions had 
been written in C code to utilize the sonars and record data from them. However, when we 
attempted to use the existing sonar system, we found that it was both hard to understand 
and extremely fragile. Indeed, we never managed any success with the existing system, 
failing on several attempts to acquire any usable data. At this point, the decision was made 
to pursue the design and implementation of a reliable and easily used sonar system for 


Yamabico. 


B. Functional Goals of the Sonar System 

Sensors can be used in two cognitively different ways. First, they provide information 
about the surrounding environment about which the perceiving entity was previously 
unaware. In the robotic world we are considering, this might translate to the detection of 
previously unmapped obstacles in the robot’s path. These sensor detections are unplanned 
events whose occurrence is outside the control of the robot. Their detection, however, can 
be strongly influenced by the scanning routines employed by the robot and by inference 


methods employed to determine the probability of such events. 


On the other hand, sensors can be employed to verify conditions in the surrounding 
environment about which the robot is already aware. For example, if the robot is directed 
to traverse a corridor it is aware, either explicitly or implicitly, that there are two walls 
parallel to it's intended direction of travel. The explicit knowledge may take the form of a 
map of the building held by the robot, while the implicit knowledge is inherent in the 
concept of "corridor". In either event, the robot knows that it may measure the environment 
and compare the result of those measurements against it's internal representation of the 
environment in order to assess it’s situation. 

Our goal is to develop a sonar system which provides the user with the functionality 
to explore and use the environment in the manner described above. It would be an easy 
matter to simply say “look everywhere all the time and record everything you see" in order 
to provide a complete and continuous catalog of Yamabico's environment. The 
computational resources necessary to accomplish such a task, however, far exceed the 
capabilities of our machine. We must be much more definitive about where we look, when 
we look and what we do with the data once we've sensed it. From another perspective, the 
sensory are a basic task for the robot, just as are the locomotion functions for physically 
moving itself about. We wish to keep these basic functions on the same level of complexity, 
providing a homogenous environment for the eventual user of the robot control language. 
For this reason, complex actions such as an automatic safety sweep of the surrounding area 
are left to higher level implementations which will use the more basic functions provided 


here. 


C. Design Goals 


Our basic design goals may be described as follows: 


1) Provide basic sonar data (range and position) with the minimum delay 
possible. 

2) Provide a method of linear feature extraction for the description of the robot’s 
environment (presumed, for our purposes, to be orthogonal). 

3) Minimize the use of CPU time as much as possible. 

4) Maximize the autonomy of the sonar system, thus distributing the processing 
to some degree. 


6) Reduce the complexity of the hardware system in order to improve it's 
reliability and speed. 
5) Provide a user friendly interface in keeping with existing MML functions. 


D. Thesis Organization 

Subsequent chapters of this work will address our hardware and software design in 
response to the design goals stated above. Chapter II 1s devoted to a review of other work 
related to the implementation of sonars aboard mobile robots. 

Chapter III presents the development of the hardware for our project. It begins with a 
review of what existed originally and traces it's evolution into the new architecture. 
Chapter IV discusses the extraction of linear features from the environment by means of a 
least squares fit algorithm, and goes on to describe the means by which data points are 
selected and filtered for application to the algorithm. Chapter V presents the high level 
functionality of the sonar control language, the user interface. Also discussed here are the 
data structures the user needs to be aware of in order to properly use the functions. The 
background functions which implement the user interface are discussed in Chapter VI 
along with data structures which are normally hidden from the user. Chapter VII presents 
the results of testing of the language, including some actual missions run in the corridor and 


the data returned. Conclusions and avenues for future work are presented in Chapter VIII. 
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II. RELATED WORK 


The bulk of current research in mobile robot sensors is directed towards the 
implementation of machine vision and tactile sensors. Ultrasonic rangefinding, however, is 
attractive for it's low cost and relatively simple implementation, making it an excellent 
research tool for educational institutes. It is also invaluable in environments where optical 
sensors are occluded (for example, aboard submersible robots). 

In a paper published in 1985, Crowley describes a sonar based modeling and 
navigating system for the IMP mobile robot at Carnegie-Mellon [3]. The proposed sonar 
system utilizes one transducer with a beam spread of approximately 5 degrees. The 
transducer is to be rotated in steps of 3 degrees, completing a full revolution in about 10 
seconds. The range of the sensor is reported as 25.6 feet with a resolution of 0.10 feet. 
Crowley develops his sensor model by first converting his range data into cartesian 
coordinates and then searching for "break" points. Breaks are defined as points where the 
distance between adjacent coordinate pairs differed by more than a preset constant. The 
resulting sets of points are fitted to line segments using a recursive routine which compares 
a constant to the perpendicular distance from individual points to a line drawn between the 
set's endpoints. The resulting line segments are then linked together to form the world 
model. Drumheller also conducted research with a single sonar transducer [4]. In his work 
a Polaroid transducer is mounted at an altitude of 5.5 feet and is rotated in 3.6 degree 
increments, thus a complete revolution is made in 100 steps. The greater altitude of the 
sensor over the IMP (which held the sensor at 31 inches above the floor) served to give 
Drumheller’s machine a view of the room vice a view of the furnishings. Drumheller then 
extracted line segments from the data in using an iterative endpoint fit. With the segmented 
representation of the room Drumheller performed pattern matching to determine the 
sonar’s (and thus an attached robot’s) location and orientation in the room. 

Elfes has conducted research in sonar based mapping and navigation at Carnegie- 


Mellon [5]. His work is based on the Neptune mobile robot, a three wheel device with a 


circular array of 24 Polaroid ultrasonic transducers. The transducers are separated at an 
angle of 15 degrees and are at a height of 31 inches above the ground. The Polaroid 
transducers operate at a frequency of approximately 55 KHz and have a beamwidth across 
the main lobe of roughly 30 degrees. The machine is not autonomous, having a Z80 
microprocessor aboard which simply manages the firing of the transducers. The collected 
data 1s transferred via a serial link to a VAX mainframe where the interpretation of the data 
takes place. The focus of Elfes' research is the development of an occupation probability 
map for an area based on the accumulation of unique views of individual regions. As more 
views of a region (either from different sonars or from the same sonar at different positions) 
return an echo from that region the probability of that region being occupied goes up. Also, 
the resolution of the occupancy map improves as views from different points are collected. 
Elfes relates some basic problems with the use of ultrasonic sensors, particularly: 


- sensitivity sharply declines when the axis of the sonar beam departs from the 
normal of the reflecting surface 

- sonar beams suffer from specularity, or the reflection of the beam between 
multiple surfaces, causing false range readings 

- the relatively wide beamwidth of the sonar beam imposes only a loose 
constraint on the position of the detected object 


Multiple transducer sonar on an autonomous mobile robot is one of the achievements 
of the HERMIES-IIB robot assembled at the Oak Ridge National Laboratory[1]. 
HERMIES-IIB mounts 25 Polaroid transducers. 24 of these are mounted as six 2x2 arrays 
and the remaining sensor is mounted singly as a collision avoidance system. Five of the six 
arrays are mounted in a rotatable, semi-circular ring on top of the robot; the sixth 1s 
mounted on a tiltable platform attached to the rotating ring. The advantage of the 2x2 array 
organization of the transducers is the reduction of the sonar beamwidth by virtue of phased 
array operation. Reduced beam width means, of course, enhanced resolution of object 
location. HERMIES-IIB is a major improvement in mobile computing power over earlier 
autonomous robots. It boasts an IBM AT microcomputer with both hard and floppy disk 


drives and 2 Mbytes of RAM. The IBM is host to an eight node NCUBE parallel processing 


system which executes the navigation and image processing programs (the robot also 
carries two cameras, in addition to the sonars). The [BM computer communicates toa VME 
rack by way of an eight megabaud parallel link. The VME rack is loaded with dedicated 
processors for the operation of the sonar subsystem, motion control, manipulators and other 
ancillary functions. The application of the sensor systems is interesting in that the sonar 
system is used for the purpose of navigation, and the vision system for interpreting and 
operating various mechanical systems and control panels. 

Crowley expands his earlier work into systems with multiple sensors [3]. He makes 
Strides in improving the time response of the sonar system to the user by his method of 
maintaining a "sonar horizon". He utilizes a segment finding method similar to that used in 
his earlier work, with the advantage that multiple points can be found simultaneously with 
multiple sensors. The line segments composing the local model are expressed in parametric 
terms, facilitating later matching with a world map. 

The theoretical works бу Кис [11,12] are of interest in that they present the physical 
basis for the operation of the ultrasonic sensors commonly used aboard mobile robots. Of 
particular interest is his development of a simulation model, for in that development lies 
some understanding of peculiar range data distribution at corners and edges that had been 
observed in previous work on Yamabico. 

Early work on our platform, Yamabico, was conducted by Hartman, Kanayama and 
Smith [7]. In this work, the least squares fit method for segment finding is explored and the 
sonar system used to facilitate precise navigation. The least squares fit algorithm 1s 
described in greater detail in a later work by Kanayama and Noguchi [8] and it's application 


to the NPS AUV project is detailed by Floyd, Kanayama and Magrino in [6]. 


IIl. HARDWARE DEVELOPMENT 


As pointed out in the introductory chapter, the existing sonar of Yamabico had failed. 
A functional, reliable sonar system is essential to the continuing research which used 
Yamabico as a test platform. Evaluation of the current system, our sensor needs and our 
fabrication abilities led us to the conclusion that a newly designed sonar to replace the 
existing system was the correct approach. Our design parameters included: 


- direct bus interface for data transfer 

- reduction of number of circuit cards to one (other than drivers) 
- fast enough to reduce positional uncertainty to less than 1 cm. 
- use Of existing transducers and drivers 

- ability to choose between polled and interrupt operation 


In this chapter we will first briefly describe the existing sonar system of Yamabico. 
We will then go on to describe the new control system hardware and it’s method of 


operation. 


A. Existing Sonar System of Yamabico-11 


Yamabico employs twelve ultrasonic sensors, or sonars, operating at 40 kilohertz and 


distributed around the periphery of the robot as shown in Figure 3.1 below. 





Figure 3.1. Sensor Location 


Each sensor is actually a pair of transducers, one to transmit the ultrasonic pulse and 
another to receive the echo. These sonar transducers are connected to three transmit/receive 
boards which control four sonars each. These boards amplify the oscillator signal provided 
by the 6809 processor and apply it to the sonar transmitters, and amplify the received echo 
to provide an output pulse at TTL levels. The three transmit/receive boards are in turn 
controlled by a 6809 processor board which times the output signals, gathers the return 
pulse and forwards the data. The data stream is routed via a ttl/rs232 converter to a serial 
input / output card, and thence finally to the VME bus and the central processor (see Figure 


3.2 below). 
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Figure 3.2. Existing Sonar Hardware Architecture 
The data for an individual sonar is composed by the 6809 processor and transmitted to 
the 68020 central processor in a series of four one byte serial transfers, each driven by a 
separate interrupt. The interrupt handler in the central processor concatenates the single 
bytes into one long word, which it then breaks up into the actual range data and the sonar 


number. In addition, the interrupt handler records the robots position in x, y and theta with 


the transfer of every byte, even though only the values recorded with the transfer of the /ast 
byte are placed on the stack for further use. When four bytes have been collected, the 
interrupt handler places the data on the stack and calls a C routine to update the sonar table, 
and then calls another C routine to perform linear fitting and other functions upon the data. 
When the system failed to operate, an evaluation was made to determine what the 
problem might be. As a result of that evaluation, these conclusions were reached: 


- the network of circuit cards and cables was far more complex than necessary, 
contributing to the frailty of the system. 

- if the system worked, the interrupt driven byte-wise transfer of data would 
occupy an inordinate amount of processor time, causing system delays. 

- existing software was poorly documented and relatively “unfriendly”. 


A design group consisting of Yutaka Kanayama, Sol Sherfey and Mike Williams was 
formed and determined that the construction and implementation of a new sonar system 
was both called for and within the capabilities of the group and the facilities available to 


them. 


B. New System Design 


The design goals were as follows: 


- retain the existing transducers and their driver/amplifiers. 

- reduce the circuitry between the driver/amplifiers to only one card. 

- make sonar data immediately available to the bus, vice following a complex, 
circuitous (i.e. slow) serial path. 

- reduce sampling time to a minimum. 

- allow for either interrupt driven or polled operation 

- improve coding to process data more efficiently and provide the user with a 
more intuitive programming environment. 


The software aspects of the project are detailed in the Software Implementation 
chapter. In the remainder of this section we will discuss the hardware/firmware aspects of 


the project. A block diagram of the new sonar system is provided in Figure 3.3. 
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Figure 3.3. New Sonar Hardware Architecture 


1. Sonar Grouping 


In order to reduce sampling time the sonars are operated in logical groups of four. 
The sonars of a logical group are all pulsed simultaneously and thus reduce the sampling 
time by a factor of four as compared to individual firing of the sonars. The sonars of each 
logical group are oriented in such a way as to: 


- prevent mutual interference 
- provide a "look" in all four directions from each group 
- present a similar aspect from each sonar during a rotational scan 


Thus, logical group 0 consists of sonars 0, 2, 5 and 7 (see Figure 1); group 1 of sonars 
1, 3, 4 and 6; group 2 of sonars 8, 9, 10 and 11; and group 3 is a “virtual” group which 


consists of four permanent test values. The axis of each sonar is oriented at 90 degree angles 


10 


from it's neighbors and the sonars of a group are distributed symmetrically about the 
robot's axis of rotation. 

In addition to being /ogically grouped, the sonars are also pAysically grouped. The 
physical grouping of the sonars 1s made to distribute the electrical load over the driver 
boards evenly and thus minimize any electrical transients associated with operation of the 
sonar. The physical grouping connects sonars О, 2, 8 and 11 to driver/amplifier board 1; 
sonars 4, 5, 6 and 7 to board 2; and sonars 1, 3, 9 and 10 to board 3. The reader will note 
that pairs of sonars from logical groups are assigned to physical groups, for example, sonars 
0 and 2 from logical group Ü are assigned to physical group (driver/amplifier board) 1. To 
further reduce any power transients associated with sonar operation, the paired sonars are 


pulsed in opposition to one another, as shown in Figure 3.4. 


2. Pulse Control 


Initial design of the control circuitry was based on two primary parameters: (1) a 
desired maximum range of 400 cm. and (2) a pulse width of 1 msec. Assuming a speed of 
sound in air, at sea level, of 340 meters/second we may calculate a round-trip time: 


NE 400 cm. 
= 2 = 2. . Ж z 
round trip time Ae x 2 =S msec (Eq 3.1) 


This round trip time is the period during which a valid echo may be received and is 
referred to as the receive gate. This interval is rounded up to 24 msec. and is derived by 
division of the sonar system's 2 MHz clock to ensure that the receiver is not falsely 
triggered by a direct path reception from it's adjacent transmitter, we opt to disable the 
receiver until the transmit pulse is complete. This will have the disadvantage of setting a 
minimum range equal to half the distance sound would travel in the time of a transmit pulse. 

minimum range = 34000 cm./sec. x 1 msec. x 0.5 = 17 cm. (Eq 3.2) 

This minimum range lies approximately 9 cm. outside the periphery of the robot. In 

order to allow the measurement of objects up to the periphery of the robot, the pulse width 


was decreased to 0.5 msec thus reducing the minimum range to 8.5 cm. 
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Figure 3.4. Opposed Sensor Firing 

ln actual practice, the minimum range is set by firmware to 9.6 cm., the additional 
distance being due to some time being allotted for switching and settling in the circuitry. 

All sonars of a logical group are pulsed simultaneously. Which groups are fired is 
determined by the value of the corresponding bit in the command register of the sonar 
control board, which in tum is set by the user with an MML function. Hence, if bit 2 is set 
to l then group 2 sonars will be pulsed. If more than one group is selected to be pulsed, the 
sonar control board will pulse the first group on the list, and when the data from that pulse 
has been read from the fourth data register the sonar control board will proceed to the next 
group and pulse it, and so on in round robin fashion. Groups with their control bit set to 0 
will not be pulsed. The sampling rate can thus be as high as 41 Hz with only one group 
enabled (based on a 24 msec. read gate as determined in equation 3.1) and will be halved 
for each additional group enabled. At a nominal robot speed of 30 cm/sec this sampling rate 
could provide an updated range within 0.75 cm. of travel, exceeding our desired positional 


accuracy of 1 cm. Of course, real performance will be affected by any delay in reading the 


data registers due to other demands on the central processor (processing the sonar data, 


controlling motion, etc.). 


3. Range Finding 

There are four 16 bit data registers on the sonar control board, one for each of the 
four sonars in a logical group. When the transmit pulse is sent to the driver/amplifier boards 
à counter is started which increments each of the data registers every 6 microseconds. This 
time period is equivalent to a range of 1.02 millimeter: 

range = 340000 mm/sec x 6 microsec x 0.5 = 1.02 mm (Eq 3.3) 

The incrementation of a particular data register continues until an echo 1s received 
or the range gate times out. The first 12 bits of the data register are allotted for range 
accumulation, thus allowing for a maximum range of 4.177 meters (4095 x 1.02 mm). If 
the range gate should time out before an echo is received, the high bit of the over ranged 
sonar's data register is set to 1. This is the “overrange” bit and is used to signal the ensuing 
software that no echo was received. Bits 12, 13 and 14 of the data registers are not used. 
When the ranging cycle is complete, the appropriate group number is written into bits 4 and 
5 of the status register and the "ready" bit, bit 7 of the status register, is set to 1. The ready 


bit is used as a flag when operating in the polled mode; i.e. without interrupts. 


4. Interrupt Control 


The sonar control board is actually a daughtercard which rides on a VME bus 
mothercard. The mothercard carries address decoders, bus drivers and interrupt control 
circuitry in the Bus Interface Module (BIM). 

When the sonar has completed a ranging cycle an interrupt request is provided to 
the BIM. The BIM’s control register holds information which determines whether an 
interrupt is to be generated or not, and if so which interrupt level is to be generated. 
Presuming an interrupt is generated, when the correct acknowledgment returns on the 
address lines the BIM's vector register provides the vector table entry where the central 


processor may find the vector to the interrupt handler. The correct interrupt level, the 
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interrupt enable bit and interrupt vector are loaded to the BIM during software 
initialization. 
S. Data Transfer 

Each of the data registers is individually addressed on the VME bus by a VME 
short address, as is the status register. Transferral of the data is extremely straightforward. 
The interrupt handler simply reads the correct register, masks out the unwanted bits and 
writes the data to the stack. When the last data register is read, the sonar system resets the 
data registers and commences a ranging cycle on the next sonar group in it’s round robin. 


The system will continue to operate autonomously until all the sonars are disabled. 


IV. LINEAR FEATURE EXTRACTION 


In addition to simple range and point position data, we desire the sonar system to 
develop representations of /inear features in an orthogonal world. To do so we must 
provide some method for recognizing sets of data points which form the linear feature and 
a method for finding and describing the line segment that best fits that set of data points. 
This is accomplished in reverse fashion, i.e. we presume the data we are receiving belongs 
to such a set and continuously modify a descriptive line segment to a best fit of the data 
using a least squares fitting algorithm. This line segment continues to grow until the 
incoming data or certain measures of the line segment indicate that the line segment should 
be ended and a new one started. We use an implementation of least squares fitting described 


by Kanayama and Noguchi [8]. 


A. Least Squares Fitting 
Suppose we have collected n consecutive valid data points in a local coordinate 
system, (p,...., p,), where p; 7 (x, y;) fori 2 1,...,n. We obtain the moments my, of the set of 
points 
n 
т = y xi. (O <j,k <2, and j+ k €2) (Eq 4.1) 
= 


Notice that moo 7 n. The centroid C is given by 


C= E a) CH (Eq 4.2) 
The secondary moments around the centroid are given by 
2 mio Y 
M, = ру (x; - M) * — mg— EJ (Eq 4.3) 
— тт 
Mus (x; — h.) Oiu) =mi- Cem | (Eq 4.4) 
iz] 
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i=l 


mo 


We adopt the parametric representation (r, &) of a line with constants r and a. If a 
point p = (x,y) satisfies an equation 
r= xcosa+ysina (= соп) (Eq 4.6) 
then the point p is on a line L whose normal has an orientation œ and whose distance from 
the origin is r (Figure 4.1). This method has an advantage in expressing lines that are 
perpendicular to the X axis. The point-slope method, where y = mx + b, is incapable of 


representing such a case (m = оо, b is undefined). 


L 


„р = (х, у) 


r Y 
residual 


Origin X 
Figure 4.1. Representation of a line L using r and a. 
The residual of point p; = (x;, y;) and the line L = (г,0) is x;cosa + y;sin@ = r. 


Therefore, the sum of the squares of all residuals is 


n 
у = > (r — x;cosa — y;sina) ? (Eq 4.7) 


iM 
The line which best fits the set of points is supposed to minimize S. Thus the optimum 
line (r,&) must satisfy 


dS dS 
n iy oe Eq 4.8 
dr da Eq ) 
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Thus, 


di _ 2 y (r — x; COS Qt — y;sinQX) (Eq 4.9) 
dro E | n 
j 
n n n 
= 2 > ] — Pu - POIL 
і= 1 i=] і= 1 
= 2 ("тоу – т0С050 — mg, sina) 
=0 
and 
mio Moi . . 
Е OSA sina = созо + Цц sine (Ед 4.10) 
тү) moo T ` 


where r may be negative. Substituting r in Equation (4.7) by Equation (4.10), 


Sz > ( (x; Hj) cosa + (miti) sina)? (Eq 4.11) 


iz] 


Finally, 


= = 22 (хол caso: (y; - Mj) sina) (- (x; - M) sinat * (y; — B,) cose) 


pe 
n n 
= 2 D: (O - = (х= De sina cosa + 2 D (c S y = Hy) (cos^a — sina) 
i=1 i=] 
= (Мо = Му) sin2a+2M,,cos2a (Eq 4.12) 
=0 
Therefore 


atan (2M ../ (Mn. ¬ M 
E сигис (Eq 4.13) 


Equations (4.10) and (4.13) are the solutions for the line parameters generated by a 


least squares fit. 
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B. Thinness Testing 
The equivalent ellipse of inertia for the original n points is an ellipse which has the 
same moments around the center of gravity. M major and M minor are moments about the major 


and minor axes respectively (Figure 4.2). 


Y 


Major axis 


Minor axis 
Origin X 
Figure 4.2. The equivalent ellipse of inertia for line L. 
M major = (Ma + Mg) /2- | (Mo, — My) 2/4. + M2, (Eq 4.14) 
M ninor = (Moy +Mo9)/2 +f (Mop — Map) °/4 + M2, (Eq 4.15) 


The diameters dmajor On the major axis and dminor ON the minor axis of the equivalent 


ellipse are 


4 IM / Moo (Eq 4.16) 


major minor 


л т (Eq 4.17) 


We define p, the ellipse thinness ratio, to be the ratio of dminor and dmajor: 


da 
р = eet (Eq 4.18) 





major 

A small p means a thin ellipse; as p increases toward 1 the ellipse degrades to a circle 
representing a thick line or a “blob” of points. We will use p as an additional measure of 
the linearity of a set of points and, by comparing p to a constant C3, we may use p to 


determine the end of a line segment. 


C. Finding Endpoints 
The residual of a point p; 7 (x;, y;) is 
ола ТОЗИ (M, 7 yj) sing (Eq 4.19) 
Therefore, the projection, p'; of the point p; onto the major axis is 
p'; = (х,+ б,со$@, у,+ 6.sing) (Ед 4.20) 
We will use p'; and p, as estimates of the endpoints of the line segment L obtained 


from the set p of data points. 


D. Residual Testing 

In addition to the ellipse thinness testing which occurs after a new point has been 
included in the line segment, we wish to do some pre-filtering of the data in order to remove 
points from the data stream which are clearly not colinear with the existing points of set p. 
In this way we can often detect the end of a line segment before having to perform the 
considerable computations necessary to include it in the line. If the point satisfies 


O. шахс СТ, Со) (Eq 4.21) 


i+] 
where C1 and C2 are positive constants (typically, C1 = 2.0 and C2 = 2.0) and the standard 
deviation O is 


o- И Уг =?) (Eq 4.22) 


minor 


then the point can be included in the current line segment. 


E. Beginning Line Segments 


Clearly, at least two data points must be collected in order to define the start of a line 
segment. In the software model adopted in this project, the usefulness of data points to the 
current line segment is judged on a "best two out of three" basis. In this model, two out of 
three consecutive data points must fail the residual testing in section D above in order for 
the system to end the current line segment. In keeping with this model, we have chosen 


three as the number of points necessary to start a line segment. 
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With the line segment established, collection and testing of the fourth data point can 
proceed. If the data point passes the residual testing, the moments and test values for the 
line are calculated including the new point and the ellipse thinness test performed. Should 
that test pass, the line segment parameters (endpoints, length, etc.) are updated and the 
system proceeds to gather a new data point. 

If, however, the fourth data point should fail the residual testing, the system deletes the 
first data point gathered and restarts the line segment with the second, third and fourth data 
points. This process continues until the first data point collected following line segment 
initiation passes residual testing. In this way we eliminate erratic start-up data, but start the 


line segment with the earliest possible acceptable data. 


F. Ending Line Segments 


There are three ways in which a line segment is ended. It may be ended by the failure 
of data points to pass the residual testing, by the failure of the line segment to pass ellipse 
thinness testing, or explicitly ended by the user of the program. 

In the case of ellipse thinness testing, p is compared to a constant C3. If p is greater 
than C3, the line segment is ended. 

In the case of residual testing, we wish to protect ourselves from the effects of 
infrequent erroneous data points. If we were to end a line segment on the strength of one 
data point that failed residual testing, a noisy environment would quickly reduce linear 
features to an unmanageably large number of segments. As a protection against this 
Occurrence, we require that two out of three consecutive data points fail to pass residual 
tesung before ending a line segment. 

If a data point fails residual testing, the system does nothing but store the data point 
temporarily and then gathers two more data points. If either one of the second pair data 
points fails residual testing, the line segment is ended and the failed data points form the 


start of a new line segment. If both of the second pair of data points pass residual testing, 
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the errant data point is thrown out and the two succeeding data points are sequentially fed 


to the linear fitting algorithm.The process then carries on normally. 


2] 


V. SONAR USER INTERFACE 


With the physical design of the sonar system settled and the basic capabilities of the 
system defined, we turn our attention to the implementation of the sonars functions in 
software. In this chapter we will first examine our basic precepts for the design of such a 


software system, and follow that with synopsis of the functions written to support our goals. 


A. Precepts 

The user interface must clearly represent the high level tasks the user wishes to 
accomplish and transmit the correct instructions to the mid level code to perform those 
tasks. Those high level tasks include: 


1) providing the range to an object on demand 

2) providing global x,y coordinates for sonar returns on demand 
3) developing a representation of surfaces it may encounter 

4) providing those surface representations on demand 

5) recording desired data for download and analysis 


The sonar system must accomplish these tasks in a real time environment using it's 
single onboard processor, which must also handle all locomotion processing and any higher 
level functions the user may ultimately develop. Clearly our design must minimize the 
processing required whenever possible - it is not feasible to simply perform ail the 
functions all the time. Our language must, therefore, provide a method for enabling 
functions as needed and disabling those functions when the user no longer requires them. 

We must also design a language that is similar enough to the existing MML to form 
a "seamless" programming environment for the user. Not only does this mean that the sonar 
functions should be similar in format to the MML locomotion functions, but they should be 
similar in scope. The user should feel that he is accomplishing the same level of control 
with the sonar functions as he is with the locomotion functions. As an example, it would be 
appropriate to issue a sonar command to gather data from a given sonar following a 
locomotion command to move from one point to another. It would be inappropriate to issue 


a sonar command to “map the enclosing space” following a list of move commands to 
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traverse the space. A command with such wide scope is better suited to higher level 
cognitive functions, such as a navigator, rather than the sensory level. 

Finally, the user interface should reveal only the level of detail necessary for the user 
to effectively employ the sonars. Underlying processes and data structures should remain 
hidden as much as possible, leaving function calls that are as simple and easy to understand 


as possible. 
B. Sonar User Language Overview 


1. Range and Position 

In order to reduce the time needed to obtain a range value and to avoid firing all 
the sonars, an enable_sonar function is provided. After a sonar group is selected by this 
function, a background process repeatedly fires the sonar group at a constant interval and 
maintains the range values for the four sonars in the group in a data Structure called 
sonar_table. These current range values may be individually requested by a sonar function 
call, or the robot user may call the wait_sonar function to delay further processing until a 
new range update is available from the sonar. A disable_sonar function allows the user to 
turn off a sonar group when it is no longer needed, thus allowing more frequent sampling 
of the remaining enabled sonars. 

While a simple range is useful for tasks such as obstacle avoidance and wall 
following, developing a sense of position in the real world requires that we fix the position 
of those sonar returns in some sort of coordinate system. To accomplish this we provide a 
global function call, which returns a data structure called posit containing the global x and 
y coordinates of the origin of the latest sonar echo, and the orientation of the sonar axis with 


respect to the global x axis at the time of the range. 


2. Linear Features 
If the user desires to acquire a surface rather than simply a range, the 
enable linear fitting function may be called. When invoked for a particular sonar, this 


function will cause the best straight line fit for a continuing sequence of range values from 
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that sonar to be found. When the range data falls outside of preset bounds, the linear feature 
being generated will be terminated and a new segment begun. The completed linear feature 
is described in a data structure called a segment which, in turn, is stored in a segment list 
for that sonar. A disable linear fitting function allows the user to discontinue linear fitting 
when not needed. A set parameters function allows the user to specify certain parameters 
for the least squares line fitting routine used to generate the linear features. A 
finish. segments function allows the user to complete segments at the end of a particular 
motion of the robot, when no other indication exists to cause the ending of the segment. 
As noted above, completed segments are stored in a list of such structures for each 
sonar. The user may access these structures with a get. segment function call, which returns 
the segment at the head of the list and moves the head pointer to the next descriptor. The 
descriptor returned is the oldest descriptor on the list; successive calls to get segment 
produce successively more recent descriptors until the most recent is sent and the head 
pointer goes to null. If the user needs the data for the linear feature currently being 
assembled by the background process, a function called get current. segment is provided. 


The need for such a function arises from the need for current data for navigational updates. 


3. Data Logging 

Since Yamabico is, after all, a research vehicle it will be necessary for the robot 
to communicate what it measures with it's sonars back to a host machine and the user. This 
operation is accomplished in two steps. First, the data to be logged is selected and the data 
logging enabled by the enable data logging function. The data is stored in arrays 
according to the type of data selected for logging. These arrays are the raw data log, the 
global data log and the segment data log. There are four of each type of data array, for 
a total of twelve data files. At the end of the robot's mission the collected data may be 
transferred to the host by use of the xfer raw to host, xfer global to host, and 
xfer segment to host functions, respectively. Since the sonar system can collect data at 


intervals as short as 25 milliseconds, it may be desirable (or necessary) to record only a 
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portion of the data collected. To accomplish this the ser log interval function is provided. 
Of course, as with the other enabling functions, data logging can be stopped with the 


disable data logging function. 


4. Program Control 


When the user writes his program for the robot, he may find that he needs to 
"stall" the progress of the program at times to allow the robot to physically “catch-up” to 
the real world position that the program presumes it is in. This is necessitated by the method 
of operation of the locomotion functions. As sequential locomotion functions (ex. move, 
rotate, and stop) are encountered in the user program, they are processed onto a queue and 
the processing of the user program continues. The function at the head of the queue is 
performed until it's goal has been met, at which time it is popped off the queue and the next 
sequential function is commenced. The result is that although the user writes a sonar 
function after, say, the third move command in his program, in actuality it may occur during 
the execution of the first command. To counter this effect, the designers of the locomotion 
functions included mark motion and wait, motion commands to halt further processing of 
the user program until a specific sequential command was complete. In our sonar language, 
we incorporate a similar feature to halt processing of the user program until certain 
conditions are met. This is the wait until command, which can delay processing based on 
the robot's x, y, or theta or based on the range from a given sonar. Additional functions 
available to the user to assist in control of the robot are the enable апа 
disable_interrupt operation functions. With these functions the user can shift from 
interrupt driven operation to polling operation and back, thus allowing the user greater 


latitude in shaping the operation of the robot. 


C. Data Structures Synopsis 


All of the data structures used in sonar.c are of a fixed size and are established at 
compile time. Since the functions always deal with the same data structures, differentiating 


only by sonar number within the structures, those structures were made global in scope and 
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parameter passing is often limited to a single integer representing the sonar number. All of 


the data structures are defined in mml.h in such a way that the structures are actually 


declared in main.c and referred to as external in other files, including sonar.c. In the 


remainder of this section we will present a brief description of all the data structures 


specific to sonar.c. and of interest to the user. 


1. Sonar Table 


Structure: 


Description: 


typedef struct{ 
int file[3], 
fitting, 
global, 
update, 
interval, 
filenumber[3]; 
double d, 
X, 
у, 
С 
EX, 
БУ, 
offset, 
phi, 
axis; 
) SONARD; 
SONARD sonar_table {16}; 


As can be seen from Figure 6.1, the sonar table is central to the 
operation of the sonar system. It contains not only the range (d) 
but the robot’s position at the time of the range (x, y and t) and 
the global coordinates corresponding to that range and position 
(gx and gy, if global conversion is enabled). The sonar table is 
also the location of constants describing the position of the indi- 
vidual sonar relative to the robot’s coordinate system (offset, 
the euclidean distance from robot center to sonar center; phi, the 
angular offset from the robot’s x-axis to the sonar center; and 
axis, the angular orientation of the sonar beam’s axis to the 
robot’s x-axis). The sonar table also contains a number of flags 
which guide the operation of the sonar system. These are file[3] 


and filenumber[3], which hold information for selecting and 
logging data; interval, which describes how often to log raw or 
global data; fitting, which directs the linear fitting of data; 
global, which directs the global conversion of data; and update, 
which informs the sonar system that new data exists in d. 

An array of sixteen of these structures is formed, which is then 
indexed by sonar number. 


2. Segment Descriptors 


Structure: 


Description: 


typedef struct{ 
int sonar; 
double headx, 
heady, 
tailx, 
taily, 
phi, 
r, 
length, 
dmajor, 
dminor; 
LINE SEG; 
LINE_SEG seg_list[16][5]; 
LINE_SEG segstruct; 
int seg list head[16]; 
int seg list. tail[16]; 


The LINE SEG structure contains all the data necessary to 
completely describe a line segment. This includes an integer to 
represent the sonar which recorded the segment, and doubles to 
record the endpoints (head x and y, tail x and y), the angle and 
length of a normal to the segment from the origin (phi and r), 
the length of the line segment and the length of the axes of the 
ellipse containing all the data points of the segment (dmajor and 
dminor). These structures are arranged in a two dimensional 
array. One index is the number of the sonar from which the 
segment is derived; the other index has two pointers, seg list. 
head and seg. list. tail. The nth position of each of these arrays 
holds an integer (0 through 4) which points to a position in the 
nth array of seg list. By using these pointers a circular queue 

is formed of each of the arrays of seg list which can hold the 5 
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9: 


Data Logs 


Structure: 


Description: 


most recent segments described by a given sonar. It is presumed 
that any navigation program will not require more history than 
these five segments; if so, the second index of seg list can be 
increased. The individual LINE SEG structure called segstruct 
Is a temporary storage location used by get current segment and 
end segment. 


typedef struct( 

int count, 
next; 

double — darra[MAXRAW], 
xarray[MAXRAW], 
yarray[MAXRAW], 
tarray[MAXRAW]; 
} RAW; 

RAW raw_data_log[4]; 


typedef struct { 

int count, 
next; 

double xarray[MAXGLOBAL], 
yarray[MAXGLOBAL]; 
} GLOBAL; 


GLOBAL global_data_log[4]; 


typedef struct{ 

int count, 
next; 

double LINE SEG array[MAXSEGMENT|]; 
) LINES; 


LINES segment data. log[4]; 


The data logs are arrays to which the user program writes data 
during it's execution. These logs are converted to ASCII strings 
at the completion of the user program by the xfer data to host 
functions, and those strings are in turn transferred to the host by 
the host xfer function. There are three types of data logs, the 
raw. data log, the global data log and the segment data log, 
which are referred to by their respective type numbers Û, 1 and 
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4. 


Posit 


Structure: 


Description: 


2 when using the enable data logging function. For each log 
type there are four structures, or data files, in the respective 
array. These are referred to by their respective file numbers 0, 
1, 2 and 3 in the enable data logging function. The size of the 
arrays within the structures limits the number of data elements 
that may be logged in each datafile. These array size values 
MAXRAW, MAXGLOBAL and MAXSEGMENT are defined 
in mml.h. The count value is the number of data values reported 
to the logging function, while the next value is the number that 
have actually been logged (these values will differ if 

log. interval is set to a value other than 1). The raw data log 
records range and the robot's x, y and t positions at the time of 
the range. The global data, log records global x and y values for 
sonar returns. The segment data log records line segments in 
the form of segment descriptors previously described. 


typedef struct{ 

double gx, 
gy. 
pst; 
)posit 


This structure is used to pass the global coordinates of a sonar 
return and the orientation of the sonar axis with respect to the 
global x axis back to the requesting function. 


D. Definitions Synopsis 


There are some definitions made in the mml.h which may make the programming of 


some of the functions easier. These mnemonics may be used in place of the integer 


parameters called for by the function synopsis. 


l. 


Sonar Numbers 


FRONTL 
FRONTR 
LEFTF 
LEFTB 
RIGHTF 
RIGHTB 
BACKL 


— ON ئ ل‎ ++ uU. © 
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BACKR 2 
LFRONTLEFT ll 
BACKLEFT 8 
BACKRIGHT 9 
FRONTRIGHT 10 
TESTOVERFLOW 2 
TEST1024 13 
TEST2048 14 
TESTDELAY 15 


2. Wait Until Parameters 


X (7 

M 13 

A 14 

DO () 

DI | 

D2 2 

D3 3 

D4 4 

D5 S 

D6 6 

D7 7 

D8 8 

D9 9 

DIO 10 
DII B 
GT 15 

Lil: 16 
EQ 17 

PI 1 2 3.14159265358979323846 
DPI DAT 
HPI 0.5 n 
PI34 OTST 
PI4 0.25 x 


E. User Function Synopsis 
Complete code of the sonar user functions is contained in Appendix A. For each 


function we provide here a synopsis of the functions syntax and a brief description of what 


the function accomplishes. Where an integer n is specified as a parameter to indicate a sonar 


number, the user may instead use predefined values such as FRONTL or BACKRIGHIT. 


These definitions are made in mml.h and will be presented in the next section, Data 


Structures. Other such definitions exist and will be noted in the appropriate synopsis. 


Enable Sonar 


Syntax: 


Description: 


Disable Sonar 


Syntax: 
Description: 
Sonar 
Syntax: 


Description: 


Wait Sonar 


Syntax: 


Description: 


Global 


Syntax: 


Description: 


vold enable_sonar(n) 

int n; 

Causes sonar n to pulse and receive echoes. Range data 
recorded in sonar. table. User should remember that all the 
sonars in the logical group to which sonar n belongs will pulse 
simultaneously. 


void disable sonarí(n) 

int n; 

Turns off sonar n. User should remember that the group will 
continue to pulse if any other sonars in that group are enabled. 


double sonar(n) 

int n; 

Returns the latest range value for sonar n in centimeters as a 
floating point number. Returns -1.0 if sonar was over-ranged 
and 0.0 if range was less than 10 centimeters. 


double wait. sonar(n) 

Int n; 

Causes processing to wait until a new value of range for sonar 
nis received, then returns range as in sonar(n). 


posit global(n) 

int n; 

Returns the structure posit, which contains global x and y 
values for the origin of the last sonar echo and sonar axis 
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10. 


orientation at time of range, for sonar n. 


Enable Linear Fitting 


Syntax: void enable linear. fitting(n) 
int n; 
Description: Causes linear fitting routine to find best straight line through the 


set of data points collected from sonar. Stores segments in 
seg list, which can store up to five segments. See the seg list 
Synopsis. 


Disable Linear Fitting 


Syntax: void disable linear. fitting(n) 
int n; 
Description: Stops the linear fitting of data for sonar n. 


Enable Data Logging 


Syntax: void enable data logging(n,filetype,filenumber) 
int n, filetype, filenumber; 
Description: Sets the correct file flag for sonar n to cause a particular data 


type to be logged into a file designated by filenumber. A filetype 
value of RAW will cause raw sonar data to be logged, GLOBAL 
will cause global coordinates to be logged, and SEGMENT 
will cause segments to be logged. There are four files for each 
filetype, labeled as 0, 1, 2 and 3. For example: 

enable data logging(RIGHTF,GLOBAL,2); 
will cause global coordinates for sonar 7 to be logged in file 
number 2. The integer values of RAW, GLOBAL and 
SEGMENT are defined in mml.h. 


Disable Data Logging 


Syntax: void disable data logging(n,filetype) 
int n, filetype; 
Description: Stops the logging of data filetype for sonar n. 


Get Segment 
Syntax: LINE SEOG *get segment(n) 
Int n; 
Description: Returns a pointer of type LINE_SEG to the oldest segment in 


array seg. list for sonar n. Function is destructive; i.e. it will 
move the head pointer to the next segment in seg. list when 
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11. 


12. 


13. 


14. 


15. 


it returns the pointer to the oldest segment, thus successive 
calls to this function will return subsequent segments until 
seg. list 1s empty. If get segment is called on an empty list 
a null pointer will be returned. 


Get Current Segment 


Syntax: LINE SEG *get current segment(n) 
Int n; 
Description: Returns a pointer to the segment currently under construction 


if there is one, else returns a null pointer. It does this by calling 
end segment and returning a pointer to the temporary data 
structure that end. segment constructs. The line segment is not 
ended by this function call - it will continue to grow until the 
linear fitting algorithm determines it should stop. This function 
merely takes a "snapshot" of the segment as it exists at the 
moment. 


Set Parameters 


Syntax: void set, parameters(c1,c2,c3) 
double c1, c2, c3; 
Description: Allows the user to adjust constants which control the linear 


fitting algorithm. C1 is a multiplier for standard deviation and 
C2 1s an absolute value; both are used to determine if an 
individual data point is usable for the algorithm. C3 is a value for 
ellipse thinness; it is used to determine the end of a segment. 
Default values are set in main.c to 3.0, 5.0 and 0.1 respectively. 


Enable Interrupt Operation 


Syntax: void enable interrupt. operation() 
Description: Places the sonar control in the interrupt driven mode, which is 
the default mode. 


Disable Interrupt Operation 


Syntax: void disable. interrupt. operation() 

Description: Causes the sonar control board to cease generating interrupts. 
Bit seven in the status register is set when there is data 
ready, and it is the user's responsibility to poll the system. 


Set Log Interval 


Syntax: void set log interval(n,d) 
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16. 


17. 


18. 


19. 


20. 


Description: 


int n, d; 

Sets the value of interval for sonar n to d. This causes the data 
logging function to record one data point for every d points sent 
by sonar n. Effective for raw and global data only, has no effect 
on the logging of segments. Default value is 13, which for a 
speed of 30 cm/sec and a sonar sampling rate of 40 Hz (one 
group enabled) results in a data point every 10 cm. 


Xfer Raw To Host 


Syntax: void xfer_raw_to_host(filenumber,filename) 
int filenumber; 
char *filename; 

Description: Causes raw data from a file filenumber to be downloaded to 
the host. Filename must be entered in double quotes 
("dumpraw" for example). 

Xfer Global To Host 

Syntax: void xfer global to host(filenumber,filename) 
int filenumber; 
char *filename; 

Description: Same as xfer raw to host, but for global data vice raw data. 


Xfer Segment To Host 


Syntax: 


Description: 


void xfer segment to host(filenumber,.filename) 

int filenumber; 

char *filename; 

Same as xfer raw. to host, but for segment data vice raw data. 


Finish Segments 


Syntax: 


Description: 


Wait Until 


Syntax: 


void finish segments(n) 

Int n; 

Completes segments for sonar vn at the end of a data run. 
Necessary because the linear fitting function only terminates a 
segment based on the data - it has no way of knowing that the 
user has stopped collecting data. 


void wait until(variable,relation,value) 
int variable, relation; 
double value; 
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Description: 


Function will delay it’s completion (and thus the continuance 
of the program it’s embedded in) until the variable achieves 
the relation with the value specified. For example, presume 
the robot is traveling along the X axis. If the user wants the 
robot to begin producing sonar data when the robot’s x position 
exceeds 500 cm., he would insert this command after the move 
command: 

wait. until(X,GT,500.0); 

enable. sonar(sonar. number); 
The variables are predefined as X, Y, A, and DO through D11, 
and correspond to the robot's x position, y position, theta, and 
range from sonars 0 through 11. Relations are predefined as 
GT, LT and EQ corresponding to greater than, less than and 
equal to. Value may be any number expressed as a double or 
the predefined values PI, HPI, PI34, PIA or DPI. 
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VI. BACKGROUND SOFTWARE IMPLEMENTATION 


With both the high level and low level interfaces established, we now turn our 
attention to the mid level code that forms the bulk of the software system. This code 
manipulates the data provided by the low level hardware as directed by commands from the 
high level functions. The results of these manipulations are either made directly available 
to the user or are logged in memory for later transfer to the host computer. We will first 
examine the precepts for the mid level design, then present an overview of system operation 


and a synopsis of the background functions. 


A.  Precepts 

The background software system must perform the work specified by the user through 
the high level commands upon the raw data provided by the hardware (low level) system. 
This work, or mid level tasks, includes: 


1) interpreting the high level commands 

2) controlling the hardware/software system 
3) gathering the correct data 

4) processing the data into the desired forms 
5) returning data as requested 

6) storing data when directed 


As pointed out in the last chapter, Yamabico has only a single processor with which to 
perform all of the computation for the robot. It is essential, therefore, that we design our 
code to be as conservative as possible with processor time. We pursue this goal along two 
avenues. First, we selectively process the data rather than universally processing it. By this 
we mean that we perform only the calculations necessary to provide the data the user 
specifically asks for, rather than performing calculations over an entire set of data. As an 
example, rather than routinely calculating global coordinates for all sonar returns, we 
perform those calculations only for those sonar returns designated for linear fitting, global 
data logging or return of global coordinates to the user. Second, we must design functions 


in such a way as to minimize processing under “normal” conditions and degrade to more 
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lengthy procedures as it's operating environment becomes more complex. As an example, 
the linear fitting algorithm should loop through the minimum code possible while the input 
data is all continuously acceptable. If the robot's sonar data becomes less reliable, then 
additional testing of the data should be performed only as long as required to filter errant 
data points from the input stream. When the data again becomes routinely acceptable, the 
processing should return to the minimum level possible. 

We must also account for the interrupt driven nature of the machine in the design of 
our software. It would be a simple matter to program a machine which had only to deal with 
sonar processing for one sonar, but a significantly more complex problem to deal with 
twelve sonars on a time shared basis with locomotion and I/O processes. Using the linear 
fitting algorithm as an example once again, we note that we must preserve the value of 
several summations between calls to the sonar system which provide the data points, and 
must do so for each sonar we are evaluating. In a single purpose processor this interim 


storage would not be necessary. 


B. Background System Overview 


The functions discussed in the previous chapter are the user functions - the functions 
normally available to the user for writing his programs. To support these user functions 
there are a set of background functions which the user would not normally use or even have 
access to. At the root of these background functions is the sonar interrupt handler, called 
ih sonar. This program is written in assembler code and is the interface between the sonar 
hardware and the sonar software system. When the hardware has data ready and generates 
an interrupt, ih. sonar first saves the state of the processor. It then places the sonar data on 
the stack, determines which of the sonars are over-ranged and places that data on the stack, 
places data about the robot's position on the stack and calls serve sonar. When serve sonar 
returns control to ih. sonar, the processor is returned to the state it was in before the 


interrupt and the interrupt processing 1s complete. 
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Serve sonar serves two major purposes. First, it loads the correct range data into the 
sonar table along with the robot's positional data and sets a flag indicating that the data for 
that sonar has been updated. Second, it examines a number of flags and determines which 
functions must be performed on which data. The scheduling which occurs here is the first 
step in paring the processor's work load to the minimum amount necessary to accomplish 
the user's desires. 

If serve sonar determines that global coordinates are required for a given sonar, it 
calls the calculate global function which will perform the calculations and load the 
coordinates into the sonar. table for that sonar. Next, it determines whether the user desires 
line segments to be derived from the data. If so, it calls the linear fitting function. 
Linear fitting in turn uses several subroutines, Including start segment to gather the initial 
points for the line segment, add to line to add data points to the line segment after it’s 
initial formation, and end_ segment to terminate the line segment when deemed necessary. 
The build list function is used to add the completed segment to the proper list, and the 
reset accumulators function is used when necessary to reset the summations for the linear 
fitting algorithm. 

When the various data conversions are complete for a given sonar, serve sonar 
determines whether the user desired for any of the data to be logged, and if so calls the 
log data function which extracts the correct data and places it in the appropriate array. At 
the completion of the mission, the host xfer function is used to transmit the data as a single 
string to the host computer. 

The reader will note one additional function in the sonar.c file. That is msbn, which is 
the target of an interrupt handler ih _msbn. Msbn is the location of a future precise 
navigation system, and as such will doubtless be part of it’s own source file. For the interim, 
msbn 1s included in sonar.c as a placeholder for ih _nsbn. 

Figures 6.13, 6.1b and 6.1c on the following pages give a graphic representation of the 
sonar system functions and their basic interrelations. The background functions are 


arranged on the left side of the diagram and the user functions on the right side. Key data 
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structures are displayed in the middle. Sonar data flow is represented by heavy lines, while 


control and selection values are represented by the light lines. 


C. Data Structures 
The following are additional data structures which are hidden from the user. 
Knowledge of these data structures is necessary only to understand the functioning of the 


background system. 


1. Segment Data 


Structure: typedef struct { 

int n, 
rst, 
sgmp; 

double initx(3], 
inity[3], 
sgmx, 
sgmy, 
sgmx2, 
sgmy2, 
sgmxy, 
sgm, delta sq, 
theta, 
r, 
d major, 
d minor, 
Startx, 
Starty, 
endx, 
endy; 
) CUR DATA; 

CUR DATA segment data[16]; 


Description: the operation of the linear fitting algorithm is dependant on the 
accumulation of certain sums. Since the processor 1s not dedi- 
cated to the linear fitting process for a given sonar, we must be 
able to save these summations in some interim state between 
iterations of the linear fitting algorithm. This is the purpose of 
the sgmx, sgmy, sgmx2, sgmy2, sgmxy and sgm delta sq items 
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in CUR. DATA. In addition, we save the uncorrected starting 
position and ending position of the segment (startx, starty, endx 
and endy) and some parameters of the segment that are updated 
for every iteration (theta, r, dmajor and dminor). The integers 
represent the sonar number (n), the number of data points thus 
far included in the line segment (sgmp) and a number 
indicating which state the linear fitting algorithm should return 
to for the next iteration of this line segment (rst). The structures 
are arranged as a sixteen wide array indexed by sonar number. 


2. Miscellany 


There are some other variables and pointers mentioned that deserve some 


explanation. These include: 
-service_flag 


-enabled_sonars| 16] 


-command_ptr 


-enabled 


-Status_ptr 


-BIM_ptr 


an integer variable used to track the number of 
enabled processes. If service_flag = 0 then no 
processes (global conversion, linear fitting, etc.) are 
active and the serve_sonar function can short circuit 
a large portion of code. 


indexed by sonar number. A value of 1 at a position 
would indicate that sonar is in use. 


a pointer to the command register on the sonar 
control board. This is a write only register; the value 
of the command register may not be read. 


a variable that contains the current value of the 
command register. This is where the contents of the 
command register must be read. 


a pointer to the status register of the sonar control 
board. This is a read only register and is at the same 
address as the command register. Therefore, if an 
attempt is made to read the command register vice 
enabled, the contents of the status register is what 
will be returned. 


a pointer to the BIM control register. The BIM is 


the Bus Interface Module on the VME motherboard 
which carries the sonar control board. 
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-group. array[4][4] this array maps sonar numbers to groups. 


CI CDC 


constants for linear fitting algorithm. Declared in 
mml.h, initialized in main.c. 


D. Background Function Synopsis 


While the user sonar functions are those the user would employ in his/her program, 


the background sonar functions are those functions the system employs to control the sonar 


system and achieve the users objectives. Code for the interrupt handler is provided in 


Appendix B, and code for the “C” functions is provided in Appendix A. A brief synopsis 


of the functions are provided here. 


l. 
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Interrupt Handler 


Syntax: 
Description: 


Serve Sonar 


Syntax: 


Description: 


ih sonar 
this is an assembly code program which resides in interrupt.s. 
When the sonar control board’s interrupt is acknowledged by 
the processor, the BIM provides a vector to the starting address 
of this program. This program then accomplishes the following 
tasks: 

- saves the state of the processor and coprocessor 

- loads the sonar group number onto the stack 

- loads the sonar data registers onto the stack 

- composes the overflow word and loads it onto the stack 

- loads robot's current x,y,t onto the stack 

- calls the serve sonar routine 

- upon return, restores the processor and coprocessor to 

their previous state 


void serve, sonar(x,y,t,ovfl,data4,data3,data2,datal,group) 
double x, y, t; 

int ovfl, data4, data3, data2, datal, group; 

this function is the “central command” for the control of all sonar 
related functions. It is linked with the ih_sonar routine and loads 
sonar data to the sonar_table from there. It then examines the 
various control flags to determine which activities the user 
wishes to take place and calls the appropriate functions. This 
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3. 


4. 


function is invoked only when a sonar group is enabled, causing 
the sonar control board to generate interrupts. If one sonar 
group is enabled, this function is invoked approximately every 
25 milliseconds. 


Calculate Global 


Syntax: 


Description: 


Linear Fitting 


Syntax: 


Description: 


void calculate global(n) 

int n; 

calculates the global x and y coordinates of the point located by 
sonar n. Uses the range value for sonar n, the position and 
orientation of the robot at the time the range was acquired, and 
the dislocation of the sonar from the center of the robot. All of 
this data is located in sonar. table[n]. Function first permutes the 
posture of the robot to find the global posture of the selected 
sonar, using the phi, offset and axis values from the sonar table. 
The global x and y position of the sonar is then translated along 
the axis of the sonar beam the distance d, resulting in the global 
x and y values for the echoing point. These are stored as gx and 
gy in the sonar table. See Figure 6.2. 


void linear fitting(n) 

int n; 

controls the fitting of global coordinate data to straight line 
segments. First collects three data points to initialize a line 
segment (see start segment). After the segment is established 
the procedure tests each subsequent point for it's acceptability 
prior to adding it to the segment (see add to line). After 
including the data point the segment is tested to ensure the entire 
set of data points is “linear enough”. If two out of three points 
are unacceptable or if the set of points fails linearity checks the 
line segment is ended and a new one started (see end segment). 
The completed line segment data is recorded in a data structure 
called segment and the segment 1s added to a two dimensional 
array called seg. list. Seg list is indexed by sonar number and 
pointers to the oldest and newest segments stored at that sonar 
number. Function uses a multivalue flag rst to track the status 
of the line segment and the three latest points being fitted. See 
the Linear Feature Extraction chapter for more details 
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Figure 6.2. Robot and Sonar Position Values. 


Start Segment 
Syntax: void start segment(n) 
int n; 
Description: establishes a new line segment with the three data points con- 


tained in segment. data[n].init(x and y). It writes the appropriate 
data to the interim values in segment. data[n]. 


Add To Line 

Syntax: void add to. line(n,x,y) 
int n; 
double x, y; 
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10. 


Description: 


End Segment 


Syntax: 


Description: 


calculates new interim data for the line segment and stores it in 
segment data[n]. Input parameters x and y are the global x and 
y coordinates of the point being added. The function also 
calculates the thinness ratio for the segment and updates 
segment parameters if the ratio is satisfactory, otherwise it 

sets flag rst to indicate that the segment failed thinness and must 
be ended. 


LINE SEG *end segment(n) 

Int n; 

calculates the true endpoints of the line segment and it's length. 
The data 1s written into a temporary buffer and a pointer to that 
buffer is returned. 


Reset Accumulators 


Syntax: 


Description: 


Build List 


Syntax: 


Description: 


Log Data 


Syntax: 


Description: 


void reset accumulators(n) 

int n; 

resets the accumulative values in segment_data[n] (sgmx, 
sgmy, sgmx2, sgmy2, sgmxy) to zero. 


void build list(ptr,n) 

Int n; 

LINE_SEG *ptr; 

Stores the segment that ptr points to in the array seg_list at the 
next position (pointed at by the tail pointer) and then updates 
the tail pointer. If the tail pointer rolls around to the location of 
the head pointer (seg list is essentially 16 circular queues) it 
will also move the head pointer up one position. This destroys 
the oldest segment in the queue and keeps the five newest. 


void log data(n,type,filenumber,1) 

int n, filenumber, type, i; 

causes data to be written to a file. The filenumber specifies to 
which of the four files (0, 1, 2, 3) for a given data type the data 
will be written to. The value of i 1s used to index the seg list 
array for storing line segments. 
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ll. Host Xfer 
Syntax: void host. xfer(buffer, filename) 
char *butter; 
char *filename; 
Description: transfers a data string to the host. 
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VII. TEST RESULTS AND CONCLUSIONS 


Testing was conducted in two phases. In the first, or static, phase the robot was 
supported on blocks and the operating parameters of the sonar were tested. In the second, 
or dynamic, phase the locomotion functions were called into play. The robot was first 
allowed to run a mission while on the blocks, to ensure the interoperability of the sonar and 
locomotion systems. The robot was then run in the hallway of Spanagel Hall to gather real 


world data for download to the host system, a Sun 3/60. 


A. Static Testing 

The first trial program (Appendix C.1) simply asked the user for a sonar number and 
then displayed the range returned by that sonar repetitively, with a delay set by a number 
of wait_sonar commands. The second program (Appendix C.2) was only slightly more 
complex and allowed for the user to enable or disable multiple sonars. Using these 
programs, we were able to determine that each of the sonars did indeed work and return a 


correct range, and we were able to determine the beamwidth of the sonar. 


1. Sonar Operability 

Table 7.1 lists the results of the individual tests of the sonars at a range of 
approximately 500 millimeters. The a and b values are the distances from the receiver 
transducer and the transmitting transducer to the target, respectively, as shown in figure 
7.1.While we expected to find ranges equal to the average of these two values, the actual 
ranges more closely followed the target - receiver distance. The average error was +18.95 
mm., placing the average sonar centroid very near the location of the receiver transducer. 
The greatest error noted was +32.5 mm., or 6.5% of the indicated range of 504 mm. range 


for sonar 9. 
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Figure 7.1. Transmitter - Receiver Offset 


Table 1: Sonar Ranging Test Results 
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2. Effective Beamwidth 

In the next portion of the static testing, we used a movable target wall to 
experimentally determine the effective beamwidth of the sonar. The theoretical beamwidth 
of a sonic transducer in the far field may be calculated as 

0 = (1.22A)/D (Eq 7.1) 

where 0 is in radians, A is the acoustic wavelength and D is the transducer diameter. For 
our operating frequency of 40 KHz and with a transducer diameter of 1.5 cm., we calculate 
a theoretical beamwidth of 0.697 radians, or 40 degrees. We expect this value to be reduced 
somewhat by the beam shaping cones placed around each transducer. Also, we note that we 
are actually measuring conditions under which enough sonic energy is returned to the 
receiver to exceed the threshold value necessary to register a return pulse. This will form 
an “effective beamwidth” which is much less than the theoretical beamwidth and dependent 
on transmitter signal strength, receiver sensitivity, air conditions and the nature of the 


reflecting surface. Out test setup is depicted in Figure 7.2. 
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Figure 7.2. Sonar Beam Test Setup 
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The leading edges of the two panels were insinuated into the sonar beam, first 
individually and then simultaneously, to determine the point at which usable sonar 
information was returned. For our purposes, usable sonar information was interpreted to 
mean at least 50% of the returns actually provide a range, rather than an over-range (no 
return) signal. 

When the leading edge of panel A was inserted to the midpoint of the receiver’s 
axis, 50% of the sonar pulses produced a return. Those returns were of range 103 
centimeters. Similarly, the insertion of panel B to the midpoint of the receiver’s axis 
produced a 50% return rate with a range of 103 centimeters. With both panels inserted to 
the midpoint of their respective transducers axis, a 100% return rate was achieved with a 
range value of 100 centimeters. This variation can be explained by examining the 


transmitted and received pulse’s waveshapes, shown in Figure 7.3. 


threshold 








transmit receive 





A B 
Figure 7.3. Sonar Waveshapes 


While the return shown actually begins at point A, and the elapsed time at point 
A would produce an accurate range, the sonar does not detect the return until point B, when 
the return pulse exceeds the receiver threshold. If we presume that the return pulse is barely 
sufficient to exceed the threshold, as it must be if we achieving only a 50% return rate, and 
knowing that our transmit pulse width is 500 microseconds, we may calculate that an 
additional 250 microseconds or 8.58 centimeters are added to the round trip distance. This 
maximum delay would be 4.29 centimeters of actual range and is, indeed, the value 


indicated when the return rate drops to 1046 or less as the panel is withdrawn. 
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The 100 centimeter range returned with both panels inserted to the axis midpoints 
indicates that the return signal is exceeding the threshold much earlier than it was with a 
single panel. This would be expected, as the reflecting surface is increased in area thus 
causing more sonic energy to be returned to the receiver. In keeping with this theory, we 
find that if the two panels are pushed completely together, forming a continuous reflecting 
surface, the indicated range drops to 99 centimeters. 

As can be seen from Figure 7.2, the transducer axes are displaced from the center 
of the transducer pair by 22.5 millimeters. This distance, at 1 meter range, equates to an 
angle of only 1.3 degrees, thus forming an effective beamwidth at 1 meter of only 2.6 
degrees. This very narrow beamwidth will permit precise location of surface edges when 
applied in an orthogonal world,and also makes the sonar very sensitive to surface 
orientation. If the surface is not within a very few degrees of perpendicular to the sonar 


beam’s axis, the reflected pulse will not fall within the sonar’s receive cone and will be lost. 


B. Dynamic Testing 


Initial testing of the sonar system in combination with the locomotion system were 
conducted with the robot supported on wooden blocks. In addition to ensuring the two 
systems would run together concurrently, these tests allowed the author to learn how to 
write user programs that actually accomplished the intended goal. Because the sequential 
locomotion functions are placed on a queue as they are encountered in the user program, 
the user must learn when to delay further execution of the user program so that the robot 
may physically “catch up” to the program. Learning these techniques with the robot simply 
spinning it’s wheels rather than lurching about the hallway was a great timesaver. 

Once the programming techniques were learned, a simple program was written that 
had the robot proceed for 6 meters while recording global position data and deriving and 
recording line segments descriptive of that data (Appendix C.3). The venue for the tests was 
Spanagel Hall, fifth deck at Naval Postgraduate School and included two doorways that 


served as markers for the output data (Figure 7.4). The robot was started at the same point 
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for each data run, and two graphs display the results of each run. The left hand graph is a 
display of the individual data points in global x and y coordinates, the right hand graph is 
a display of the line segments derived by the robot to fit the data points. The robot 
proceeded along a simple DR (dead reckoned) path to collect the data, the direction being 
determined by the robot's initial heading. Deviation from a path parallel to the measured 


wall accounts for the data's tendency to "lean" one way or the other from vertical. 


Direction of travel 


| recording sonar 
Doorway Doorway 
Ó meters ——— — е 


start stop 
Figure 7.4. Sonar Dynamic Testing Venue 


]. Test One 


In this first test, the linear fitting parameters Cl, C2 and C3 were set at 3.0, 5.0 
and 0.1 respectively. This means that individual data points would be acceptable for 
inclusion in a line segment if they were displaced from the line segment by less than the 
maximum of either 3.0 times sigma or 5.0 centimeters. The line segment will be terminated 
if it’s thinness ratio exceeds 0.1. The data interval for this test, and for the ensuing tests, 1s 
set to 5. As can be seen in Figure 7.5, the global coordinate pairs returned by the sonar 
accurately describe the wall and doorways being mapped. The linear fitting of the line 


segments, however, leaves much to be desired. 
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Figure 7.5. Sonar Dynamic Test One 


Line segment AB incorporates two features, the doorway and the ensuing wall. 
Line segment CD likewise incorporates a doorway and the ensuing wall. These segments 


must be broken up into their two components in order to be useful. 


2. Test Two 
The offset from the walls to the doorway surfaces in our test area is approximately 


9 centimeters. We judged that the C2 parameter of 5.0 was too large to allow clear 
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detinition of features of that size, so for the second test run we lowered C2 from 5.0 to 2.0 


centimeters. The results of that test run are shown in Figure 7.6. 
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Figure 7.6. Sonar Dynamic Test Two 
In this case, line segments BC, CD and DE accurately represent the latter half of 
the data run. The first half, however, incorporates the first doorway and both the leading 


and trailing wall portions in one line segment, AB. Another adjustment is necessary. 


3. Test Three 
For the third test we opted to change parameter Cl, the sigma multiplier, from 3.0 
down to 2.0. Parameter C2 remains at 2.0 and parameter C3 at 0.1. The results are shown 


in Figure 7.7. 
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Figure 7.7. Sonar Dynamic Test Three 


Results from this test are not substantively different from test two, with the 
exception that the incorrect line segment, DE, has endpoints which coincide with breaks in 


the pattern of data points. This is still unsatisfactory. 
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4. Test Four 
For the next iteration of the test we turn our attention to parameter C3. Parameters 
C1 and C2 are already adjusted as small as we judge is practical, while the linearity of the 
data points suggests that a thinness factor of one in ten may be overly generous. We 
adjusted the parameter C3 down to 0.08, so that our parameters are now 2.0, 2.0 and 0.08 


for C1, C2 and C3 respectively. The results are shown in Figure 7.8. 
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Figure 7.8. Sonar Dynamic Test Four 


This combination of parameters for the linear fitting algorithm has produced 
perfect results. The doorways are clearly defined and the length and orientation of the 


segments match the sensed environment exactly. 
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C. Conclusions 
The sonar system designed for and implemented aboard Y amabico achieves the goals 
set for it. Specifically, it: 


- provides basic sonar data on demand 
- extracts linear features from the data 
- adapts to user demands to minimize processor time 
- at the lowest level, operates completely autonomously 
- has reduced hardware complexity to a great degree 
- provides a friendly interface to the user 
There are still some shortcomings to the system, however, that will require further 


work to resolve. Most notable are the problems with the effective beamwidth of the 
individual sonars. The extremely narrow beamwidth leaves large areas of the sonar azimuth 
uncovered, thus eliminating any possibility of detecting objects that appear in the 
uncovered areas between beams. The most probable fix for this problem 1s an increase in 
sonar transmitting power, although a different transducer may also effect a significant 
improvement. 

Another significant problem is the relatively small amount of memory available to the 
processor (one megabyte). Logging sonar data of large areas for later download, or creating 
onboard maps of the environment, will require substantially more memory than is currently 
available. While volatile RAM may be increased to accommodate this need for additional 
memory, it is the author’s opinion that conversion to an onboard processing system with 
it’s own operating system supporting some sort of mass storage would better serve current 
and future computational requirements. The incorporation of higher level processes, such 
as a navigator to utilize the sonar data now generated or the future installation of a vision 


system, will certainly require more onboard computing power. 
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APPENDIX A - CODE FOR SONAR *C" FUNCTIONS 


/* sonar.c */ 
/* ultrasonic rangefinder funcuons */ 
#include “mml.h” 


#define print. flex(x,y) y 2 putstr(“ “, putsu(rtoae((double) (x), umpstr, 4), y)) 
#define nl_flex(x) x = putstr(\n”, x) 


/*declaration of functions and return values*/ 


extern double sonar(); 

extern void enable_sonar(); 

extern void disable_sonar(); 

extern void msbn(); 

extern double wait. sonar(); 

extern posit global(); 

extern void enable linear. fitting(); 
extern void disable linear. fitting); 
extern void enable. data logging); 
extern void disable data logging; 
extern void serve, sonar(); 

extern LINE SEG *get segment(); 
extern LINE SEG *get current. segment); 
extern void set, parameters(); 

extern void enable interrupt. operation(); 
extern void disable interrupt. operation(); 
extern void calculate. global(); 
extern void linear_fitting(); 

extern void start, segment); 

extern void add to. line(); 

extern LINE SEG *end segment); 
extern void reset. accumulators(); 
extern void build list(); 

extern void log. data(); 

extern void set. log. interval(); 
extern void wait. unul(); 

extern void xfer raw to host(); 
extern void xfer. global, to. host(); 
extern void xfer segment to host(); 
extern void host. xfer(); 

extern void finish, segments(); 
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ПАТИО ИИТИИ УНО IO UU TOR RO o md 


/* 
/* Procedure: sonar(n) 

/* 

/* Description: returns the distance (in cenumeters) sensed by the 
/ n thultrasonic sensor. If no echo is received, then a -1 is 

/* returned. If the distance is less than 10 cm, then a O is 

/ returned. 


p 


ED UL EERE Le ee ee eee eee ey 


double sonar(n) 

int n; 

[ 

return (sonar_table[n].d); 


) 


n aa COE e EEE TEES AT SL TATE ee ee A OO KORR Y 


Е 

/* Procedure: enable sonar(n) 

/* 

/* Description: enables the sonar group that contains sonar n, which 
/* causes all the sonars in that group to echo-range and write data 
/* to the data registers on the sonar control board. Marks the n’th 
/* position of the enabled sonars array to track which sonars are 
/* enabled. 

/* 
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void enable_sonar(n) 
int n; 


[ 


Int 1; 
i = imaskoff(); 
enabled sonars[n] = 1; 
switch (n) 
[ 
case 0: 
case 2: 
case 5: 
case 7: 
enabled = enabled | 0x01; 
break; 


61 


case 1: 

case 3: 

case 4: 

case 6: 
enabled = enabled | 0x02; 
break; 

case 8: 

case 9: 

case 10: 

case 11: 
enabled = enabled ! 0x04; 
break; 

case 12: 

case 13: 

case 14: 

case 15: 
enabled = enabled | 0x08; 
break; 

) 

*command pir - enabled; 

imaskon(1); 


| 
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/* Procedure: disable sonar(n) 

ү 

/* Description: removes the sonar n from the enabled sonars list. If 
sonar n is the only enabled sonar from it's group, then the 
group is disabled as well and will stop echo ranging. This has 
benefit of shortening the ping interval for groups that remain 
enabled. 
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void disable_sonar(n) 
int n; 
{ 
int i,c; 
char mask; 


i = imaskoff(); 


enabled. sonars(n] 2 0; 
switch (n) 
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[ 


case 0: 
Gase 2: 
case 5: 
case 7: 
c - enabled. sonars(0] * enabled sonars(2] + 
enabled sonars(5] * enabled. sonars(7]; 
if (c 22 0) enabled 2 enabled & Oxfe; 
break; 
case |: 
case 3: 
case 4: 
case 6: 
c = enabled sonars(1] * enabled. sonars(3] 4 
enabled sonars(4] * enabled. sonars(6]; 
if (c 22 0) enabled = enabled & Oxfd; 
break; 
case 8: 
case 9: 
case 10: 
case 11: 
c = enabled_sonars[8] + enabled_sonars{9] + 
enabled sonars[10] * enabled. sonars(11]; 
if (c 22 0) enabled = enabled & Oxfb; 
break; 
case 12: 
ease 13: 
case 14: 
case 15: 
c 2 enabled sonars(12] * enabled sonars(13] 4 
enabled sonars(14] 4 enabled, sonars[15]; 
if (c == 0) enabled = enabled & Oxf7; 
break; 
| 


*command pu - enabled; 


imaskon(i); 
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/* Procedure: msbn 


/* Description: called every 5 ms, this routine drives the precise 


navigation system. 


63 


/* 
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void msbn() 


[ 
) 
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/* 

/* Procedure: wait. sonar(n) 

/* 

/* Description: waits in a loop until new data is available for 
/* sonar n. 

/* 
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double wait. sonar(n) 
int n; 
{ 


int a 2 0; 


sonar. table[n].update 2 0; 
while (sonar. table(n].update == 0); 
return(sonar. table[n].d); 
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/* Procedure: global(n) 

/* 

/* Description: returns a structure of type posit containing the global 
/* x and y coordinates of the position of the last sonar return. 
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posit global(n) 
int n; 
( 


posit answer; 


if (sonar. table[n].global 22 0) calculate. global(n); 
answer.gx = sonar_table[n].gx; 
answer.gy = sonar_table[n].gy; 
answer.psi 2 sonar. table[n].t + sonar_table[n].axis; 
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retum answer; 


| 
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/* 
/* Procedure: enable_linear_fitting(n) 

jt 

/* Description: causes the background system to gather data points 
/* from sonar n and form them into line segments as governed by 
/* thelinear fitüng algorithm. Increments service. flag. 

jt 
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void enable linear fitting(n) 

int n; 

{ 
sonar_table[n].fitting = 1; 
sonar_table[n].global = 1; 
++service_flag; 
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/* 
/* Procedure: disable linear fitting(n) 

/* 

/* Description: causes background system to cease forming line 
/* segments for sonar n. Decrements the service. flag. 

/* Will also disable the calculation of global coordinates for 
/* that sonar if data logging of global data is not enabled. 

P 
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void disable linear. fiting(n) 

int n; 

{ 
sonar_table([n].fitting = 0; 
if (sonar_table[n].filetype[1] == 0) sonar_table[n].global = 0; 
--service_flag; 


| 
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fe 
/* Procedure: enable_data_logging(n,filetype,filenumber) 
F 
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/* Description: causes the background system to log data for sonar (n) 

to a file (filenumber). The data to be logged is specified by an 

integer flag (filetype). A value of 0 for filetype will cause raw 

sonar data to be saved, 1 will save global x and y, and 2 will 

save line segments. The filenumber may range between 0 and 3 for 

each of the three types, providing up to 12 data files. Example: 
enable data logging(4,1,0); 

will cause raw data from sonar #4 to be saved to file 0, while: 
enable_data_logging(7,2,0); 

will cause segments for sonar #7 to be saved to file 0. 

Function increments the service_flag. 
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void enable data logging(n,filetype,filenumber) 
int n, filetype, filenumber; 
[ 
1Í (filetype == 1) sonar_table[n].global = 1; 
sonar table[n].filetype[filetype] = 1; 
sonar_table[n].filenumber[filetype] = filenumber; 
++service_flag; 


| 
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/* Procedure: disable data logging(n,filetype) 

as 

/* Description: causes the background system to cease logging data of a 
/* given filetype for a sonar n. Decrements the service_flag. 


ibs 
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void disable data logging(n,filetype) 

int n,filetype; 

[ 
if ((filetype 22 1) && (sonar. table[n].fitting 22 0)) sonar_table[n].global = 0; 
sonar  table[n].filetype[filetype] 2 0; 
--service flag; 


| 
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ig 
/* Procedure: serve_sonar(x,y,t,ovfl,datal ,data2,data3 ,data4 group) 
E 
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/* Description: this procedure is the “central command” for the 
control of all sonar related functions. It is linked with 

the ih_sonar routine and loads sonar data to the sonar_table 
from there. It then examines the various control flags in the 
sonar_table to determine which actvities the user wishes to 
take place, and calls the appropriate functions. This procedure 
is invoked approximately every thirty milliseconds by an 
interrupt from the sonar control board. 
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void serve_sonar(x,y,t,ovfl data4,data3 data2,data | ,group) 
double x,y,t; 

int ovfl,data4 ,data3 ,data2,datal ,group; 

[ 


Int 1; 
int data[4]; 
int ovfl mask 2 8; 


data[0] 2 datal; 
data[1] 2 data2: 
data[2] = data3; 


data[3] = data4; 
for (i = 0; i < 4; i++, ovfl_mask /= 2) 


if (ovfl_mask & ovfl) 
sonar_table[group_array[group][i]].d = -1.0; 
else if (data[i] < 100) 
sonar_table[group_array[group][i]].d = 0.0; 
else sonar. table[group. array[group][1]].d = (float)data[i] / 10.0; 
sonar, table[group. array[group][i]].x 2 x; 
sonar table[group. array[group][1]].y = y; 
sonar. table[group  array[group][1]].t 2 t; 
sonar table[group. array[group][1]].update = 1; 


if (service. flag != 0) 
for (1 = 0; 1 < 16; i4) 


if (Sonar_table[i].update == 1) 
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if (sonar. table[i].global == 1) 

calculate global(i); 
if (sonar. table[i].fitting == 1) 

linear. fitüng(1); 
if (sonar. table[i].filetype[0] 22 1) 

log. data(i,l,sonar. table[i].filenumber(0],0O); 
if (sonar. table[i].filetype[1] 22 1) 

log. data(1,2,sonar. table[1].filenumber[1],0); 
} 


sonar_table[1].update = 0; 
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/* Procedure: get segment(n) 

/* 

/* Description: returns a pointer to the oldest segment on the linked 
list of segments for sonar n; i.e. the record at the head 

of the linked list. It is destructive, thus subsequent calls 

will return subsequent segments until the list is empty. This is 
accomplished by first copying the contents of the head record 
into a temporary record called segstruct and then freeing the 
allocated memory for the head record. The pointer returned is 
actually a pointer to this temporary storage. If get_segment is 
called on an empty list a null pointer is returned. 
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LINE_SEG *get_segment(n) 
Int n; 
( 

LINE, SEG *ptr; 

int index; 


index = seg. list head[n]; 
if (index == -1) 
ptr = NULL; 
else 
[ 
ри = &seg list(n](index]; 
seg list head[n] 2 (index < 4) ? (index + 1) : 0; 
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retum ptr; 
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/* Procedure: get current. segment(n) 

dd 

/* Description: returns a pointer to the segment currently under 
construction if there is one, otherwise returns null pointer. 

This is accomplished by calling end. segment, copying the data 
into segstruct and then returning a pointer to segstruct. The 
memory allocated by end, segment is then freed. 
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LINE_SEG *get_current_segment(n) 
int n; 
[ 

LINE_SEG *pu; 


if (segment_data[n].rst > 1) 
pur = end_segment(n); 

else ptr = NULL; 

return ptr; 


{ккк жж ж жж жж жож жж жж жож жж жж ЖЖ ЖЖ ЖЖЖЖ ЖЖЖЖ жж жож жж жож жж жж жж жж жж жж жж жж ж жж Жжжж жж ж | 
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/* Procedure: set, parameters(c1,c2,c3) 

ү” 

/* Description: allows the user to adjust constants which control 

the linear fitüng algorithm. C1 is a multiplier for standard 
deviation and C2 is an absolute value; both are used to 
determine if an individual data point is usable for the 

algorithm. C3 is a value for ellipse thinness; it is used to 
determine the end of a segment. Default values are set in main.c 
to 3.0, 5.0, and 0.1 respectively. 
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void set. parameters(c1,c2,c3) 
double c1,c2,c3; 
{ 
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С=с: 
(20x 
C3 = c3; 
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/* 

/* Procedure: enable_interrupt_operation() 

f* 

/* Description: places sonar control board in interrupt driven mode. 


/* 
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void enable interrupt. operation() 


| 
*BIM prt 7 *BIM pir ! 0x10; 
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[* 

/* Procedure: disable interrupt. operation() 

/* 

/* Description: stops interrupt generation by the sonar control 

/* board. A flag is set in the status register when data is ready, 
/* and it is the user's responsibility to poll the sonar system 

/* for the flag. 
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void disable interrupt. operation() 


| 
*BIM ptu - *BIM pt & Oxef; 
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/* Procedure: calculate. global(n) 

/* 

/* Description: this procedure calculates the global x and y coordinates 
/* for the range value and robot configuration in the sonar table. 

/* The results are stored in the sonar table. 


[* 
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void calculate. global(n) 
int n; 


| 


n 
А 
Ы 
a 
/* 


x x x X X X X X X X X X 


double lx, ly, gt, range, phi, axis, offset; 


gt = sonar table[n].t; 

range - (float)sonar. table[n].d; 
phi = sonar. table[n].phi; 

axis = sonar. table[n].axis; 
offset 2 sonar. table[n ].offset; 


if (range == -1) range = 9999; 

lx = sonar_table[n].x + (cos(gt + phi) * offset); /* global x position of sonar */ 
ly = sonar_table[n].y + (sin(gt + phi) * offset); — /* global y position of sonar */ 
sonar table[n].gx = lx + (cos(gt + axis) * range); /* global x position of range */ 
sonar_table[n].gy = ly + (sin(gt + axis) * range); /* global y position of range */ 
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Procedure: linear. fitting(n) 


Description: this procedure controls the fitting of range data to straight 
line segments. First it collects three data points and establishes 
a line segment with it's interim data values. After the segment 
is established, the procedure tests each subsequent data point 
to determine if it falls within acceptable bounds before calling 
the least squares routine to include the data point in the line 
segment. After inclusion of the data point the segment is again 
tested to ensure the entire set of data points are linear enough. 
If any of the tests fail, the line segment is ended and a new one 
started. The completed line segment is stored in a data structure 
called segment, and segments are linked together in a linked list. 
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void linear. fitung(n) 
int n; 


[ 


int sgmp, rst; 
double theta, r, sigma, delta; 
LINE_SEG *finished_segment; 


theta - segment data[n].theta; 
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r - segment. data[n |.r; 


sgmp — 2 segment data[n].sgmp; 
rt - segment, data[n].est; 

LU (ESL mmi) 

[ 


segment. data[n].initN[sgmp] 2 sonar, table[n].g; 
segment, data[n].inity[sgmp] 2 sonar, table[n].g y: 
segment. data[n].sgmp *- l; 
if (sgmp == 2) 
[ 
start, segment(n); 
segment. data[n].rst 2 1; 
| 
| 
else 
[ 
sigma — segment, data[n].sgm. delta sq (double) sgmp; 
delta = sonar. table[n].gx * cos(theta) * sonar. table[n].gy * sin(thetaà) - r; 
if ((tabs (delta) < (sigma * C1)) II (fabs (delta) < C2)) 
| 
switch (rst) 
| 
casa 1: 
segment, data[n].sgmp += l; 
add, to. line(n, sonar. table[n].gs, sonar. table[n].g y): 
if (segment, data[n].est 22 1) 
[ 


segment, data[n].rst 2 2; 


| 

else if (segment, data[n].rst 2-7 5) 

[ 
reset. accumulators(n); 
segment, data(n].initN[O] 2 segment, data(n].initx[ 1]; 
segment, data[n].inity[O] 2 segment, data[n].inity[ 1 ]; 
segment, data[n].initN[ 1] = segment. data[n].initx[2]; 
segment, data[n].inity[ 1] 2 segment, data[n .initv[2]; 
segment, data[n].initX[2] 2 sonar, table[n].g; 
segment. data[n].inity[2] 2 sonar, table[n].gy; 
segment, data[n].sgmp = 3; 
start, segment(n); 
segment, data[n].rst 9 1: 

| 
break: 


case 2: 


segment data[n].sgmp += 1; 
add to line(n, sonar. table[n].gx, sonar, table[n].gy); 
if (segment. data[n].rst —— 5) 
( 
finished segment - end, segment(n); 
build list(finished segment, n); 
reset accumulators(n); 
segment, data[n].initx[0] 2 sonar. table[n].gx; 
segment data[n].inity[0] 2 sonar. table[n].gy; 
segment data[n].sgmp - 1; 
segment data[n].rst — 0; 


break; 
case 3: 
segment, data[n].initx[1] 2 sonar. table[n].gx; 
segment, data[n].inity[1] 2 sonar. table[n].gy; 
segment, data[n].rst = 4; 
break; 
case 4: 
segment. data[n].sgmp += 1; 
add to line(n, segment, data[n].initx[1], segment. data[n].inity(1]); 
if (segment, data[n].rst 2- 5) 
( 
finished segment 2 end, segment(n); 
build list(finished segment, n); 
reset accumulators(n); 
segment, data[n].initx[2] 2 sonar. table[n].gx; 
segment, data[n].inity[2] 2 sonar. table[n].gy; 
segment data[n].sgmp - 3; 
start segment(n); 
segment data[n].rst 2 1; 


else 
( 
segment data[n].sgmp += 1: 
add to. line(n, sonar. table[n].gx, sonar. table[n].g y); 
if (segment, data[n].rst 22 5) 
[ 
finished segment 2 end. segment(n); 
build, list(finished segment, n); 
reset accumulators(n); 
segment, data[n].initx[0] 2 sonar. table[n].gx; 
segment, data[n].inity[0] 2 sonar. table[n].gy; 
segment data[n].sgmp = 1; 
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segment, data[n].rst = 0; 
else segment data[n].rst = 2; 


break; 


else 
( 
switch (rst) 
[ 
case 1: 

case 2: 
segment_data[n].initx [0] = sonar_table[n].gx; 
segment_data[n].inity[0] = sonar_table[n].gy; 
segment_data[n].rst = 3; 
break; 

case 3: 
finished segment 2 end, segment(n); 
build, list(finished segment, n); 
reset accumulators(n); 
segment, data[n].initx[1] 2 sonar. table[n].gx; 
segment. data[n J.inity[1] 2 sonar. table[n].gy; 
segment, data[n].sgmp - 2; 
segment  data[n].rst 2 0; 
break; 

case 4: 
finished segment 2 end, segment(n); 
build. list(finished, segment, n); 
reset accumulators(n); 
segment data[n].initx[2] 2 sonar. table[n].gx; 
segment, data[n].inity[2] 2 sonar. table[n].gy; 
segment, data[n].sgmp = 3; 
start segment(n); 
segment, data[n].rst 2 1; 
break; 
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/* 
/* Procedure: start segment(n) 
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jT 

/* Description: this procedure establishes a new line segment with the three 
/* аага points contained in segment data[n].init(x and y). It writes 

/* the appropriate data to the interim values in segment data[n]. 

25 
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void start. segment(n) 

int n; 

{ 
double theta, r, mux, muy, muxx, muyy, muxy, sds; 
int 1,j; 


segment, data[n].startx 2 segment data[n].initx [0]; 
segment, data[n].starty 2 segment data[n].inity[O]; 
segment data[n].endx 2 segment, data[n].initx[2]; 
segment, data[n].endy - segment data[n].inity[2]; 
for (i = 0; 1 < 3; ++i) 
{ 
segment, data[n].sgmx -«- segment data[n].initx [1]; 
segment data[n].sgmy -«- segment, data[n].inity[i]; 
segment data[n].sgmx2 «- SQR(segment data[n].initx [1]); 
segment, data[n].sgmy2 «2 SQR(segment data[n].inity[i]); 
segment data[n].sgmxy «2 segment data[n].initx[1] * segment, data[n].inity[1]; 
) 
mux - segment data[n].sgmx/3.0; 
muy - segment data[n].sgmy/3.0; 
muxx - segment data[n].sgmx2 - SQR(segment data[n].sgmx)/3.0; 
muyy - segment, data[n].sgmy2 - SQR(segment data[n].sgmy)/3.0; 
muxy - segment data[n].sgmxy - (segment, data[n].sgmx * segment data[n].sgmy)/3.0; 
theta = (atan2( -2.0 * muxy, (muyy - muxx))) / 2.0; 
r = mux * cos(theta) + muy * sin(theta); 
for (J = 0; J < 3; ++J) 
( 
sds «2 SQR(segment data[n].initx(j] - mux) * SOR(cos(theta)); 
sds *- SQR(segment  data[n].inity(j] - muy) * SQR(sin(theta)); 
sds += 2.0 * (segment, data[n].initx[j] - mux) * (segment data[n].inity(j] - muy) 
* cos(theta) * sin(theta); 
) 
segment_data[n].sgm_delta_sq = sds; 
segment_data[n].theta = theta; 
segment data[n].r = г; 
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/* 
/* Procedure: add to line(n, x, y) 

i 

/* Description: this procedure calculates new interim data for the line segment 
/* and stores it in segment_data[n]. It also changes the end point values to 
/* the point being added. 

3 
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void add_to_line(n,x,y) 

int n; 

double x, y; 

{ 
double sgmp; 
double m, major, m minor, d major, d, minor, theta, r; 
double mux, muy, muxx, muyy, muxy, sds; 


sgmp = (double)segment_data[n].sgmp; 
segment_data(n].sgmx += x; 
segment_data[n].sgmy += y; 
segment, data[n].sgmx2 «2 SQR(x); 
segment, data[n].sgmy2 «2 SQR(y); 
segment data[n].sgmxy += x * y; 


mux - segment data[n].sgmx / sgmp; 

muy - segment data[n].sgmy / sgmp; 

muxx - segment data[n].sgmx2 - SOR(segment data[n]).sgmx) / sgmp; 

muyy - segment data[n].sgmy2 - SQR(segment data[n].sgmy) / sgmp; 

muxy - segment data[n].sgmxy - (segment data[n].sgmx*segment data[n].sgmy) / sgmp; 


m_major = (muxx + muyy)/2.0 - sqrt((muyy-muxx)*(muyy-muxx)/4.0 + SQR(muxy)); 
m_munor = (muxx + muyy)/2.0 + sqrt((muyy-muxx)*(muyy-muxx)/4.0 + SQR(muxy)); 
d major = 4.0 * sqrt(fabs(m minor/sgmp)); 
d minor = 4.0 * sqrt(fabs(m major/sgmp)); 


if ((d minor / d major) « C3) 
[ 
theta = (atan2( -2.0 * muxy, (muyy - muxx))) / 2.0; 
r = mux * cos(theta) + muy * sin(theta); 
sds += SQR(x - mux) * SQR(cos(theta)); 
sds += SQR(y - muy) * SQR(sin(theta)); 
sds += 2.0 * (x - mux) * (y - muy) * cos(theta) * sin (theta); 
segment_data[n].sgm_delta_sq += sds; 
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segment, data[n].theta = theta; 
segment, data[n].r = r; 
segment data[n].endx - x; 
segment, data[n].endy = у; 
segment. data[n].d major 2 d major; 
segment, data[n].d minor - d minor; 
) 

else segment_data(n].rst = 5; 


| 
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/* 
/* Procedure: end_segment(n) 

/* 

f* Description: this procedure allocates memory for the segment data structure, 
/* loads the correct values into it and returns a pointer to the structure. 


А 
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LINE_SEG *end_segment(n) 
int n; 
{ 
LINE_SEG *seg_ pu; 
double startx, starty, endx, endy, delta, theta, r, length; 


seg ptr = &segstruct; 


startx 2 segment. data[n].startx; 

starty 2 segment. data[n].start y; 

endx - segment data[n].endx; 

endy - segment data[n].endy; 

theta 2 segment data[n].theta; 

г  -segment data[n].r; 

delta = startx * cos(theta) + starty * sin(theta) - г; 
startx = startx - (delta * cos(theta)); 

Starty = starty - (delta * sin(theta)); 

delta = endx * cos(theta) + endy * sin(theta) - r; 
endx =endx - (delta * cos(theta)); 

endy =endy - (delta * sin(theta)); 

length = sqrt(SQR(startx - endx) + SQR(starty - endy)); 


seg ptr->headx = startx; 


seg ptr->heady = starty; 
seg_ptr->tailx = endx; 
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seg ptr->taily = endy; 

seg_ptr->phi = theta; 

seg ptr-r =fr; 

seg ptr-»length = length; 

seg pu-»dmajor - segment data[n].d, major; 
seg ptr-^»dminor 2 segment data[n].d minor; 
seg_ptr->sonar =n; 


return seg ptr; 


| 
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/* 
/* Procedure: reset accumulators(n); 

/* 

/* Description: resets the accumulative values in segment_data[n] (sgmx, sgmy, 
/* sgmx2,sgmy2, sgmxy) to zero. 

ж 


ГЕНИИ СЕУ Ык ЕКЕ ЕК ЕК ЖЕКИ КИК КК ЖУ КЫ К Б ЖОК КОКЕ К КУЫ КЫ КОН ААА АЛЛАН 


void reset accumulators(n) 

int n; 

[ 
segment_data[n].sgmx = 0.0; 
segment_data[n].sgmy = 0.0; 
segment, data[n].sgmx2 z 0.0; 
segment, data[n].sgmy2 z 0.0; 
segment data[n].sgmxy = 0.0; 


[OE КОНК ЖЖ ЖОК ЧООК КЫШЫ ЖЫШ M 


/* 
/* Procedure: build list(ptr, n); 

/* 

/* Description: this function accepts a pointer to a segment data structure and 
/*  asonar number, and appends the segment structure to the tall of a linked 
/* list of structures for that sonar. 

x 


[FRE KEKE CL O Ds 


void build list(ptr, n) 
int n; 

LINE SEG *ptr; 

( 
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int next; 


if (seg list tail[n] 22 -1) seg list head[n] 2 0; 
next = (seg list. tail[n] « 4) ? «seg. list tail[n] : 0; 
if (next == seg. list, head[n]) 
seg list head[n] 2 (seg list head[n] « 4) ? ++seg_list_head[n] : 0; 
seg list[n][next] 2 *ptr; 
if (sonar. table[n].filetype[2] 22 1) 
log data(nj3,sonar table[n].filenumber[2],next); 


| 


E Ett tU ERE ia tas bh kin ы kh kO YUK са ы ыйы са ыы ы абы ы ш ы ыйа ы чан л ыы ыбы ыбыз ы ыы ка ы | 


/* 

/* Procedure: log. data(n, type, filenumber,i) 

А 

/* Description: this procedure causes data to be written to a file. The filenumber 
/*  designates which "column" (0,1,2, or 3) of a two dimensional array for 

/* that type of data is used. The data array and a counter for each column 

/* forms the data structure for each type. The value of i is used to index 

/* theseg list array for storing line segments. 


i 


En О О УИИН И oko Ko diens So or Foro top doe oem po ee y 


void log data(n, filetype, filenumber,i) 
int n, filetype,filenumber,i; 


int count,interval next; 


switch(filetype) 
{ 
case 1: 
count = raw_data_log(filenumber].count; 
interval = sonar_table[n].interval; 
if ((count < MAXRAW) && !(count % interval)) 
{ 
next = raw_data_log(filenumber].next; 
raw data log[filenumber].darray[next] 2 sonar. table[n].d; 
raw data log[filenumber].xarray[next] 2 sonar. table[n].x; 
raw data log[filenumber].yarray[next] 7 sonar. table[n].y; 
raw data log[filenumber].tarray[next] 2 sonar. table[n].t; 
raw data log[filenumber].next «2 1; 
) 
raw_data_log[filenumber].count += 1; 
break; 
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case 2: 
count = global data log[filenumber].count; 
interval = sonar. table[n].interval; 
if ((Count « MAXGLOBAL) && !(count % interval)) 
( 
next = global data log[filenumber].next; 
global data log[filenumber].xarray [next] 2 sonar. table[n].gx; 
global data log[filenumber].yarray [next] 2 sonar. table[n].gy; 
global data log[filenumber].next += 1; 
} 
global data log[filenumber].count «2 1; 
break; 
Case 3: 
count = segment_data_log[filenumber].count; 
if (count « MAXSEGMENT) 
{ 
segment data log[filenumber].array [count] 2 seg. list[n][i]; 
) 
segment, data log[filenumber].count += 1; 
break; 


) 


нана ннн ОСТИ 
/* 
/* Procedure: set. log. interval(n,d) 
/* 
/* Description: this procedure allows the user to set how often the sonar system 
writes data to the raw data or global data files. The interval d is stored 
at sonar. table[n], and one data point will be recorded for every d data 
points sensed by the sonar. Default value for interval d is 13, which for 
a speed of 30 cm/sec and sonar sampling time of 25 msec should record a 
data point every 10 cm. 


ЗЭВ 


cec dekoeejotetekeroeotojoerekoeterekckejoclok EE EEE EEEE EEE ey 


void set_log_interval(n,d) 
int n,d; 
( 

sonar. table[n].interval 2 d; 


| 
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/1 
/* Procedure: wait unül(variable,relation,valuc) 
” 
/* Description: this procedure will delay it’s completion (and thus the continuance 
of the program it’s embedded in) until the variable achieves the relation with 
the value specified. For example, presume the robot is traveling along the x 
axis. If the user wants the robot to begin redording sonar data when the x 
position of the robot exceeds 500 cm., he would insert this command after the 
move command: 

wait until(X,GT,500.0); 

enable sonar(sonar number); 
The variable arc predefined as X, Y, A and DO through D11, and correspond to 
the robot's current x position, y position, theta, and range from sonars 0) 
through 11. Relations are predefined as GT, LT and EQ corresponding to greater 
than, less than and equal to. Value may be any numlber expressed as a double 
or the predefined values PI, HPI, PI34, PI4, or DPI. 


tet ШО ugs o VW v GS 
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void wait, until(variable,relation,valuc) 
int variable,relation; 
double value; 
{ 
double * pu; 
double theta; 
int test, iten; 


if ((variable == 14) && (relation == 17)) test 7 (int)(1000.0 * value); 
else if (relation == 17) test = (int)(value); 


switch (variable) 

{ 

case 0): 
case l; 
case 2: 
case 3: 
case 4: 
case 5: 
case 6: 


case 7: 
case 8: 
case 9: 
case 10: 
case 11: 
pu 2» &sonar table[variable].d; 
break; 
case 12: 
pu = &cur_x; 
break; 
case 13: 
ptr = &cur_y; 
break; 
case 14: 
theta = 1000.0 * cur t; 
pu = &theta; 
break; 
J 
switch (relation) 
[ 
case 15: 
do { 
item = * ptr: 
) 
while (item «- value); 
break; 
case 16: 
do( 
item = *ptr; 
) 
while (item >= value); 
break; 
case 17: 
do { 
item = (int)* ptr; 
) 
while (item != test); 
break; 
) 


/ 
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*/ 
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J” 
/* Procedure: xfer_raw_to_host(filenumber,filename) 
yt 


/* Description: this funcuon allocates memory for a buffer and then converts a raw data 


/* log file to a string format stored in the buffer. It then calls host. xfer to send 
/* — the string to the host. When that transfer is complete, it frees the memory it 
/* allocated for the buffer. Filename must be entered in double quotes ( "dumpraw" 
/* for example). 
/* 
j 
o Ox X X X 1 X XY Y O33535 YOK Y XX 33353 35 3 3 X X 3 3 3⁄ 3 3 3 3⁄ 3⁄2 5⁄2 X 3 1 3 3 3⁄ 3 3 X 3 3⁄2 3 3 X 33 жуу жу жж жж жж уу ужу HIN HH 
i 
void xfer raw to host(filenumber filename) 
int filenumber; 
char *filename; 
[ 
char *rbuffer; 
char *start; 
int 1,C J; 
i = raw_data_log[filenumber].next; 
c= 204 (i * 33); 
rbuffer = malloc(c); 
start = rbuffer; 
for G=0; j<i; j++) 
{ 
print. flex(raw. data, log[filenumber].darray[]], rbuffer); 
print, flex(raw. data. log[filenumber].xarray[j], rbuffer); 
print flex(raw data log[filenumber].yarray[]], rbuffer); 
print flex(raw data log[filenumber].tarray[]], rbuffer); 
nl flex(rbuffer); 
) 
putb(^Q' rbuffer); 
rbuffer = start; 
host_xfer(rbuffer,filename); 
free(rbuffer); 
) 
/ 
3⁄ X 3 3⁄ 3 3 X 3 3 3⁄ X 3 X 3⁄2 3 35 X 3 5 3⁄2 3⁄2 XO 3 3⁄ 3 S 5 X 3 3 3 Xy Xy X yy Xy ye oy X 3 3 Xy AO yy 3 oy X 3 X 3 3 3 3 dy ny yy yn d n n0 3 3 
"y 
/* 
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/* Procedure: xfer. global to host(tilenumber,filenamce) 

lo 

/* Description: this function performs the same function as xfer raw to host, but for 
/* global data vice raw data. 

ps 

/ 
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void xfer. global to host(filenumber,filename) 
int filenumber; 
char *filename; 
{ 
char *gbutter; 
char *start; 
int 1,C J; 


1 2 global data log[filenumber].next; 
c = 208 (í * 17у; 
gbuffer = malloc(c); 
Start = gbuffer; 
for G=0; j<1; j++) 
| 
print. flex(global data log[(filenumber].xarray[j], gbuffer); 
print. flex(global data log[filenumber].yarray (j], gbuffer); 
nl_flex(gbutter); 
} 
putb(^O', gbuffer); 
gbuffer = start; 
host_xfer(gbuffer,filename); 
free(gbutfer); 


eck vests, eRenk Re 
"y 

7" 

/* Procedure: xfer segment to. host(filenumber, filename) 

/* 

/* Description: this function performs the same function as xfer. raw. to host, but for 

/* segment data vice raw data. 


j= 


/ 
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void xfer_segment_to_host(filenumber, filename) 
int filenumber; 
char * filename; 
{ 
char *segbuffer; 
char * start; 
Int 1,C J; 


| 2 segment data log[filenumber].count; 

e=20+(i" 77); 

segbuffer = malloc(c); 

start = segbuffer; 

for (j=0; J<i; j++) 

{ 
print_flex(segment_data_log[filenumber].array[j].headx, segbuffer); 
print flex(segment. data log[filenumber].array[j].heady, segbuffer); 
print. flex(segment data log[filenumber].array[]].tailx, segbuffer); 
print flex(segment. data log[filenumber].array[j].taily, segbuffer); 
nl flex(segbuffer); 
print. flex(segment data log[filenumber].array[j].phi, segbuffer); 
print. flex(segment. data log[filenumber].array[]).r, segbuffer); 
print. flex(segment, data log[filenumber].array[j].length, segbuffer); 
print. flex(segment data log[filenumber].array[j].dmajor, segbuffer); 
print. flex(segment, data log[filenumber].array[j].dminor, segbuffer); 
nl flex(segbuffer); 
) 

putb( ^O" ,segbuffer); 

segbuffer = start; 

host_xfer(segbuffer, filename); 

free(segbuffer); 


/ 


3k okc ok oc oc ok oc ok X k ok ak ak ak ak ak ak k Ak k ok oc o oc oc ok ov ok ok o ok oc okc oc oe oc oc ok ok oc ok oc ok ok o2 oe oc ok oc ok ok oc okc oc oc ok oK ok oc oc ok o ak ak k k k Ak k k ak ak ok of oe ook ok ok ok 
| 
/* Procedure: host_xfer(buffer, filename) 


А 
/* Description: this function transfers a data string from the buffer to the host. Not a 
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/* user function; is called by data conversion functions such as xfer. raw to host. 
/* User would call the xfer raw. to host (or equivalent for global or segment data) 
/* to download data from the robot. 

ү 

/ 


* * * * * * * * k k k x Xk ak ok xk ok ok Oe X: k K k K K k K K K X k k K k X k K k k k k k k. k K X X k K k k ak ak ak ak ak ak k k k K K k R K K k K Rk R К К К К К КК KO R R k 
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void host_xfer(buffer, filename) 

char *buffer; 

char * filename; 

{ 
i_port(HOST, 9600, 0, 0, 0); 
r printf( ^12N15 connect cable and keyinV" V7); 
while(r_getchar() != ° °); 
putstr(‘\n”, HOST); 
i_port(HOST, 9600, 0, 0, 1); 
r printf(^12N5 ready for dump °); 
while(r. getchar() != ‘g’); 
putstr(^ytof ", HOST); 
putstr(filename,HOST); 
putstr(“ w \n” ,HOST); 
while(r_getchar() != ° °); 
r printf(^dumping “); 
putstr(buffer, HOST); 
putb(“4’ HOST); 
putb( M',HOST); 
r print£f( N07; 
return; 


u q aa in Tes sss 
ү 
/* Procedure: finish. segments(n) 
/* 
/* Description: this function completes segments at the end of a data run. Necessary 
/* because the linear fitting function only terminates a segment based on the 
/* data - it has no way of knowing that the user has stopped collecting data. 
D 
/ 
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void finish. scegments(n) 
int n; 
| 
LINE SEG *f(inished. segment; 


finished segment = end, segment(n); 
build list(finished segment, n); 


) 
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APPENDIX B - INTERRUPT HANDLER CODE 


BH EE B E HE BH HH IEEE HER EH EHE HERE HHEHH HR HH HH H TH IB HE HH HH FH IBHERHHREH HH 
# 

# Procedure: ih_sonar 

8 

# Description: Interrupt handler for the sonar control board. Loads 
# group number, contents of the four data registers, the overflow 

# word, current theta, current y and current x onto the stack in 

# that order. It then calls the serve_sonar function in sonar.c 

# which places the data in the appropriate data structures. Overflow 
# word is simply a four bit concatenation of the overflow bits in 

# the individual data registers. Data register contents are 

# masked to allow only the actual range data. 

# 

ч 


HHHRHHHH_RHHHHHHIHHHHHHHHHHHHHHIHHHHHHIHHIHHHHHHÑIHHIHHHHIHHHHHHIHHIHHHHRHHHHHHHHHHHEHH 
.globl 1h sonar 
status = Oxffff83£9 


datal = Oxffff83f0 
stop = Oxffff83fs 


text 

_ih_sonar: 

moveml d0-d7/a0-a5, sp@- Isave register contents before use 

link a6, 4-184 Istack space for coprocessor status 

fsave a6@(-184) lsave coprocessor status 

fmovemx fpO-fp7, fpx save sonar Isave fp registers 

fmovel fpcr, fpcr save sonar Isave fp control register 

fmovel fpsr, fpsr save sonar Isave fp status register 

fmovel fpiar, fpia save sonar Isave fp iaddr register 

ciri d0 

movl #status, a0 lload address of status register 

movb a0@, dO lload status register into dO 

andl #0x00000018, dO lextract group # from status 

Isrl #3, dO Ishift group number over to the right 

movl d0, sp@- Ipush group # onto stack 

cirl d2 

movl #datal, a0 lload address of data register #1 
xferdata: 

movw a0@, dl Imove register contents into dl 
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movl 
andl 
1511 
апа! 
tstl 
beq 
orl 


gooddata: 


addql 
cmpl 
bnc 
movl 
movl 
movl 
movl 
movl 
movl 
movl 


jsr 


add] 

fmovel 

fmovel 

fmovel 
fmovemx 
frestore 
unlk 
moveml] 
ric 


dl, sp@- 
8Ox00000fff, spa 
#1, d2 
80x00008000, dl 

dl 
gooddata 
#1, d2 


#2, a0 

#stop, a0 
xferdata 

d2, sp@- 
_cur_t+4, sp@- 
_cur_t, sp@- 
Cur. y44, sp@- 
_cur_y, sp@- 
_cur_x+4, sp@- 
cur. x, sp@- 


Serve sonar 


#48, sp 
_fpia_save_sonar, fpiar 
 fpsr. save. sonar, fpsr 

- fpcr. save. sonar, fpcr 
 fpx. save, sonar, fpO-fp7 
460 (-184) 

аб 

$р@ +, 10-47/а0-а5 
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Ipush dl onto stack 

Imask out all but last twelve bits 
Ishift overflow word left 1 bit 
lextract bit 15 of register (ovfl) 
liest for overflow 

lif no overflow branch 

Isets overflow word Isb if overflow 


lincrement address for next register 
Istop when address is ffff83f8 
Icontinuc data transfer 

Ipush overflow word onto stack 
Ipush theta onto stack. Two pushes 
| to xfer 64 bit data 

Ipush current y onto stack 


Ipush current x onto stack 


link to C routines 


Iremove parameters from stack 
irestore fp iaddr register 
Irestorc fp status register 
Irestore fp control register 
irestore fp registers 

irestore coprocessor status 
Iclear up stack 

Irestore registers 
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APPENDIX C - TEST PROGRAMS 


Operating Individual Sonars 
ГЕЕВ БИИ ЕЕРЕЕ ВЕКЕ ЕЕЕ ЕЖЕ bui LE 


read individual sonars 
August 10, 1991 by Sherfey 


ЖЖЖЖ ЖЕЕ ЕЕЕ ЕИО КЕЕ ЕЕ ЕНК ЕЕЕ Ko Eoo bo БЕК ЕЖЕ ЕГИ 


#include ““mml.h” 


user() 


| 


int n,i,d; 
Isi: 


r printf( ^12N15 input sonar no.'); 
n-getint( CONSOLE); 
enable sonar(n); 
do 
[ 
for 1=0; 1<20; 1++) wait_sonar(n); /*produces a range every half second*/ 
d = (int)sonar_table[n].d; 
if (d « 0) r. printf( ^ 12215 Overranged ); 
else 
| 
r printf( ^12N15'; 
r printfi(d); 
| 


| 
while(1==1); 
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2. 


Manipulating Multiple Sonars 
E E E EE ER E E E AER E E sc oa E a de be te ae te ke Aed OR 


manipulate sonars 
August 12, 1991 by Sherfey 


ЕЕ ЕЕН ЕЕЕ ТОЛЕ ЕВ КККК КНК ЕЕЕ Е ЖЕЖ ice A E 


#include “mml.h 


user() 


| 


int n,m,i,j, k,l,s,d,x; 


с... = 
l ll 
ju — 

ч . 


switch (]) 
| 
case 1: 
r printf( ^12M15 input number of sonar to enable: “); 
nzgetint( CONSOLE); 
enable ѕопаг(п); 
break; 
case 2: 
r printf( ^12N15 input number of sonar to disable: °°); 
m-getint( CONSOLE); 
disable sonar(m); 
break; 
case 3: 
r printf( ^12N15 input number of wait cycles: “); 
k=getint( CONSOLE); 
break; 


do 


for (s=0; s<16; s++) 


[ 


9] 


З: 


if (enabled sonars[s] 22 1) 
[ 
for (i20; i«k; i^) wait. sonar(s); 
d = (int)sonar_table[s].d; 
r prim `) 
r printfi(s); 
r_printf(“/’); 
r_printfi(d); 


) 
гарап 12315): 
х = (г getchar() !2 * )? 1:0; 
) 
while(x); 
r printf ^12N15M5 O to quit, 1 to enable, 2 to disable, 3 to change wait : “); 
jegeünt( CONSOLE); 
) 
хеб != 0); 


Test Runs In Corridor 


6. 8 2.9. 0. al aal „6 L8. Q9. L8. c6 28. 9. LE BA LR 8. L8. WM э э #1 а ө ge Ub. в OT AS FOS OS LL OI ARAB AO ON e e a a у Rep O NE 
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/* Gather global and segment data on straight run of 6 meters 
/* by Sol Sherfev August 29. 1991 


Wb ee ev ee 


ee eee ee ee ee EMEN ET T. at "wt we t at ^» t "at "WU CENE MW Fw mw e ee a Саала оа rr c 


#include ““mml.h” 


user() 


| 
POSTURE p1, p2, p3; 


def posture(0.0, 0.0, HPI, &p1); 
def posture(0.0, 600.0, HPI, &p2); 
def_posture(0.0, 650.0, HPI, &p3): 


set_parameters(2.0, 2.0, 0.08): 


set log interval(7,5); 
set rob(&pl); 


enable sonar(7); 

mark motion(); 

move(&p2); 

move(&p3); 

enable linear. fitting(7); 

enable data logging(7,1,0); 

enable data logging(7,2,0); 

wait motion(); 

disable sonar(7); 

finish, segments(7); 

disable linear. fitting(7); 
disable data logging(7,2); 

disable data logging(7,1); 

motor on - NO; 

xfer global to host(0, "test global8"); 
xfer segment to host(0," test segment8"); 


Dynamic Test On Blocks (Non-Moving) 
aa eae Re cla tl tect AE IEE AE AE E E a Ane of ot che E A eo he ce ea a ae Ae ete bre i ИВ be iS kek sea Ж / 


/* generate and record segments while rolling straight 
/* for 1000 cm and moving the barrier 
/* by Sol Sherfey 14 August, 1991 


EE EE A E a sioe ole Rak Sole coke ok eos jo k Ae k ОРЕ k A e a ie 2 o ee КОЕ КАСЕ 


#include ““mml.h”’ 
extern LINE_SEG *end_segment(); 
user() 
{ 
POSTURE p. pl, p2; 
int 1,C,Z,n; 


LINE SEG *finished segment; 


reset accumulators(7); 
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set rob(def posture(0.0, 0.0, 0.0, &p)); /* Initial posture */ 
enable sonar(7); 

move(def posture(1000.0, 0.0, 0.0, &p1)); 

enable linear. fitting(7); 

enable data logging(7,2,0); 

stop(def posture(1050.0, 0.0, 0.0, &p2)); 

do 


2 = (int)sonar_table[7].x; 
| 

while (z « 1000); 
disable sonar(7); 
finished segment 2 end, segment(7); 
build_list(finished_segment, 7); 
disable_data_logging(7,2); 
r_printff\12\1 S”); 
c —-segment data log[0].count; 
r printf( count = “); 
r printfi(c); 
r_printf( x posit = “); 
r printfi((int)sonar. table[7].x); 
r printf(* y posit 2 '); 
r_printfi((int)sonar_table[7].y); 
r printf(* range = 5); 
r printfi((int)sonar. table[7 ].d); 
r pami 6 150157): 


for (1=0; i<c; 1++) 

[ 
r_printf(“ hx =“); 
r_printfi((int)segment_data_log[O].array[1].headx); 
r_printf( hy =“); 
r_printfi((int)segment_data_log[O].array[i].heady); 
r_printf(“ tx = “); 
r_printfi((int)segment_data_log[O].array[1].tailx); 
r_printf(“ ty = “); 
r printfi((int)segment data, log[0].array[i].taily); 
r_printf{( length =“); 
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r printfi((int)segment data log[0].array[i].length); 
ГООО ЕИБ 0): 


| 


5. Another Dynamic Test Program 
ЕЕЕ Е ОЕ Е НО ЕЕК ЕЧЕИ ЕИО Е ЕЕЕ К ЕЕ Е Е 


/* generate and record segments while rolling straight 
/* test the wait. until function 
/* by Sol Sherfey 14 August, 199] 


| mana ages ee eee ee ee OR en ee ee ee ee ee | 


#include “mml.h”’ 
extern LINE SEG *end segment(); 


user() 
[ 
POSTURE p, pl, p2,p3,p4; 
Int 1,C,Z,n; 
LINE SEG *finished segment; 


reset accumulators(?); 

set rob(def posture(0.0, 0.0, 0.0, &p)); /* Initial posture */ 
enable sonar(7); 

move(def posture(300.0, 0.0, 0.0, &p1)); 
enable linear. fitting(7); 

enable data logging(7,2,0); 

wait. until(X,GT,300.0); 
disable data logging(7,2); 

disable linear. fitting(7); 
move(def_posture(400.0,100.0,HPI,&p2)); 
move(def_posture(400.0,400.0,HPI,&p3)); 
enable_linear_fitting(7); 
enable_data_logging(7,2,0); 
stop(def_posture(400.0, 450.0, HPI, &p4)); 
wait_until(Y ,GT,400.0); 

disable_sonar(7); 

finished_segment = end_segment(7); 
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build list(finished. segment); 

disable data logging(7,2); 

r_printff\12\1 1577); 

c = segment_data_log[0].count; 

г рпп(“соџп = “); 

r printfi(c); 

r_printf( x posit = “); 

r printfi((int)sonar. table[7].x); 

r_printf(“ y posit = “); 

r printfi((int)sonar. table[7 ].y); 

r printí(^ range 2 9); 

r printfi((int)sonar table[7 ].d); 

г ргіп (1 2\1 5\15”); 

ог (1=0; 1<с; 1++) 

{ 
r printf(" hx 2 5); 
r printfi((int)segment data log[0].array[i].headx); 
r printf(* hy =“); 
r pnrntfi((int)segment data log[0].array[1].heady); 
r printf(* tx = °°); 
r printfi((int)segment data  log[0].array[i].tailx); 
pinti v JF 
r_printfi((int)segment_data_log[O].array[1].taily); 
r_printf(* length =“); 
r_printfi((int)segment_data_log[O].array[1].length); 
r_printff\12\157); 
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